:

examples/pybullet/gym/pybullet_robots/panda/panda_sim.py

@@ -0,0 +1,65 @@
+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.offset = np.array(offset)
+    
+    #print("offset=",offset)
+    flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
+    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)
+    legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags))
+    legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags))
+    legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags))
+    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
+    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)
+  
+      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 step(self):
+    t = self.t
+    self.t += 1./60.
+    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)]
+    orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.])
+    jointPoses = self.bullet_client.calculateInverseKinematics(self.panda,pandaEndEffectorIndex, pos, orn, ll, ul,
+      jr, rp, maxNumIterations=5)
+    for i in range(pandaNumDofs):
+        self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.)
+    pass
+