1import time 2import numpy as np 3import math 4 5useNullSpace = 1 6ikSolver = 0 7pandaEndEffectorIndex = 11 #8 8pandaNumDofs = 7 9 10ll = [-7]*pandaNumDofs 11#upper limits for null space (todo: set them to proper range) 12ul = [7]*pandaNumDofs 13#joint ranges for null space (todo: set them to proper range) 14jr = [7]*pandaNumDofs 15#restposes for null space 16jointPositions=[0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02] 17rp = jointPositions 18 19class PandaSim(object): 20 def __init__(self, bullet_client, offset): 21 self.bullet_client = bullet_client 22 self.offset = np.array(offset) 23 24 #print("offset=",offset) 25 flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 26 legos=[] 27 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) 28 legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags)) 29 legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags)) 30 legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags)) 31 sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.6])+self.offset, flags=flags) 32 self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.5])+self.offset, flags=flags) 33 self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.7])+self.offset, flags=flags) 34 orn=[-0.707107, 0.0, 0.0, 0.707107]#p.getQuaternionFromEuler([-math.pi/2,math.pi/2,0]) 35 eul = self.bullet_client.getEulerFromQuaternion([-0.5, -0.5, -0.5, 0.5]) 36 self.panda = self.bullet_client.loadURDF("franka_panda/panda.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags) 37 index = 0 38 for j in range(self.bullet_client.getNumJoints(self.panda)): 39 self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0) 40 info = self.bullet_client.getJointInfo(self.panda, j) 41 42 jointName = info[1] 43 jointType = info[2] 44 if (jointType == self.bullet_client.JOINT_PRISMATIC): 45 46 self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) 47 index=index+1 48 if (jointType == self.bullet_client.JOINT_REVOLUTE): 49 self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) 50 index=index+1 51 self.t = 0. 52 def reset(self): 53 pass 54 55 def step(self): 56 t = self.t 57 self.t += 1./60. 58 pos = [self.offset[0]+0.2 * math.sin(1.5 * t), self.offset[1]+0.044, self.offset[2]+-0.6 + 0.1 * math.cos(1.5 * t)] 59 orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.]) 60 jointPoses = self.bullet_client.calculateInverseKinematics(self.panda,pandaEndEffectorIndex, pos, orn, ll, ul, 61 jr, rp, maxNumIterations=5) 62 for i in range(pandaNumDofs): 63 self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.) 64 pass 65 66