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