import time import numpy as np import math useNullSpace = 1 ikSolver = 0 pandaEndEffectorIndex = 11 #8 pandaNumDofs = 7 ll = [-7]*pandaNumDofs #upper limits for null space (todo: set them to proper range) ul = [7]*pandaNumDofs #joint ranges for null space (todo: set them to proper range) jr = [7]*pandaNumDofs #restposes for null space jointPositions=[0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02] rp = jointPositions class PandaSim(object): def __init__(self, bullet_client, offset): self.bullet_client = bullet_client self.bullet_client.setPhysicsEngineParameter(solverResidualThreshold=0) self.offset = np.array(offset) #print("offset=",offset) flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES self.legos=[] self.bullet_client.loadURDF("tray/traybox.urdf", [0+offset[0], 0+offset[1], -0.6+offset[2]], [-0.5, -0.5, -0.5, 0.5], flags=flags) self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags)) self.bullet_client.changeVisualShape(self.legos[0],-1,rgbaColor=[1,0,0,1]) self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags)) self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags)) self.sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.6])+self.offset, flags=flags) self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.5])+self.offset, flags=flags) self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.7])+self.offset, flags=flags) orn=[-0.707107, 0.0, 0.0, 0.707107]#p.getQuaternionFromEuler([-math.pi/2,math.pi/2,0]) eul = self.bullet_client.getEulerFromQuaternion([-0.5, -0.5, -0.5, 0.5]) self.panda = self.bullet_client.loadURDF("franka_panda/panda.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags) index = 0 self.state = 0 self.control_dt = 1./240. self.finger_target = 0 self.gripper_height = 0.2 #create a constraint to keep the fingers centered c = self.bullet_client.createConstraint(self.panda, 9, self.panda, 10, jointType=self.bullet_client.JOINT_GEAR, jointAxis=[1, 0, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) self.bullet_client.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50) for j in range(self.bullet_client.getNumJoints(self.panda)): self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0) info = self.bullet_client.getJointInfo(self.panda, j) #print("info=",info) jointName = info[1] jointType = info[2] if (jointType == self.bullet_client.JOINT_PRISMATIC): self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) index=index+1 if (jointType == self.bullet_client.JOINT_REVOLUTE): self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) index=index+1 self.t = 0. def reset(self): pass def update_state(self): keys = self.bullet_client.getKeyboardEvents() if len(keys)>0: for k,v in keys.items(): if v&self.bullet_client.KEY_WAS_TRIGGERED: if (k==ord('1')): self.state = 1 if (k==ord('2')): self.state = 2 if (k==ord('3')): self.state = 3 if (k==ord('4')): self.state = 4 if (k==ord('5')): self.state = 5 if (k==ord('6')): self.state = 6 if v&self.bullet_client.KEY_WAS_RELEASED: self.state = 0 def step(self): if self.state==6: self.finger_target = 0.01 if self.state==5: self.finger_target = 0.04 self.bullet_client.submitProfileTiming("step") self.update_state() #print("self.state=",self.state) #print("self.finger_target=",self.finger_target) alpha = 0.9 #0.99 if self.state==1 or self.state==2 or self.state==3 or self.state==4 or self.state==7: #gripper_height = 0.034 self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.03 if self.state == 2 or self.state == 3 or self.state == 7: self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.2 t = self.t self.t += self.control_dt pos = [self.offset[0]+0.2 * math.sin(1.5 * t), self.offset[1]+self.gripper_height, self.offset[2]+-0.6 + 0.1 * math.cos(1.5 * t)] if self.state == 3 or self.state== 4: pos, o = self.bullet_client.getBasePositionAndOrientation(self.legos[0]) pos = [pos[0], self.gripper_height, pos[2]] self.prev_pos = pos if self.state == 7: pos = self.prev_pos diffX = pos[0] - self.offset[0] diffZ = pos[2] - (self.offset[2]-0.6) self.prev_pos = [self.prev_pos[0] - diffX*0.1, self.prev_pos[1], self.prev_pos[2]-diffZ*0.1] orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.]) self.bullet_client.submitProfileTiming("IK") jointPoses = self.bullet_client.calculateInverseKinematics(self.panda,pandaEndEffectorIndex, pos, orn, ll, ul, jr, rp, maxNumIterations=20) self.bullet_client.submitProfileTiming() for i in range(pandaNumDofs): self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.) #target for fingers for i in [9,10]: self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL,self.finger_target ,force= 10) self.bullet_client.submitProfileTiming() class PandaSimAuto(PandaSim): def __init__(self, bullet_client, offset): PandaSim.__init__(self, bullet_client, offset) self.state_t = 0 self.cur_state = 0 self.states=[0,3,5,4,6,3,7] self.state_durations=[1,1,1,2,1,1, 10] def update_state(self): self.state_t += self.control_dt if self.state_t > self.state_durations[self.cur_state]: self.cur_state += 1 if self.cur_state>=len(self.states): self.cur_state = 0 self.state_t = 0 self.state=self.states[self.cur_state] #print("self.state=",self.state)