forked from irom-princeton/pybullet-panda
-
Notifications
You must be signed in to change notification settings - Fork 0
/
panda.py
executable file
·122 lines (102 loc) · 4.28 KB
/
panda.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
import os
import pybullet as p
import numpy as np
from numpy import array
class Panda:
def __init__(self, long_finger=False, wide_finger=False):
if wide_finger:
self.urdfRootPath = os.path.join(
os.path.dirname(__file__),
'geometry/franka/panda_arm_finger_wide.urdf'
) # Wide finger specifically for grasping cups for pouring task
elif long_finger:
self.urdfRootPath = os.path.join(
os.path.dirname(__file__),
'geometry/franka/panda_arm_finger_long.urdf'
) # longer finger from Doug's design
else:
self.urdfRootPath = os.path.join(
os.path.dirname(__file__),
'geometry/franka/panda_arm_finger_orig.urdf'
) # original finger from Franka
self.pandaId = None
self.numJoints = 13
self.numJointsArm = 7 # Number of joints in arm (not counting hand)
self.pandaEndEffectorLinkIndex = 8 # hand, index=7 is link8 (virtual one)
self.pandaLeftFingerLinkIndex = 10 # lower
self.pandaRightFingerLinkIndex = 12
self.pandaLeftFingerJointIndex = 9
self.pandaRightFingerJointIndex = 11
self.maxJointForce = [87, 87, 87, 87, 12, 12, 12] # from website
self.maxFingerForce = 20.0
# self.maxFingerForce = 35 # office documentation says 70N continuous force
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
] # joint damping coefficient
self.jointUpperLimit = [2.90, 1.76, 2.90, -0.07, 2.90, 3.75, 2.90]
self.jointLowerLimit = [
-2.90, -1.76, -2.90, -3.07, -2.90, -0.02, -2.90
]
self.jointRange = [5.8, 3.5, 5.8, 3, 5.8, 3.8, 5.8]
self.jointRestPose = [0, -1.4, 0, -1.4, 0, 1.2, 0]
self.fingerOpenPos = 0.04
self.fingerClosedPos = 0.0
self.fingerCurPos = 0.04
self.fingerCurVel = 0.05
def load(self):
self.pandaId = p.loadURDF(
self.urdfRootPath,
basePosition=[0, 0, 0],
baseOrientation=[0, 0, 0, 1],
useFixedBase=1,
flags=(p.URDF_USE_SELF_COLLISION
and p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT))
iniJointAngles = [
0, -0.1, 0, -2, 0, 1.8, 0.785, 0, -np.pi / 4, self.fingerOpenPos,
0.00, self.fingerOpenPos, 0.00
]
self.reset(iniJointAngles)
return self.pandaId
def reset(self, angles): # use list
if len(angles) < self.numJoints: # 7
angles += [
0, -np.pi / 4, self.fingerOpenPos, 0.00, self.fingerOpenPos,
0.00
]
for i in range(self.numJoints): # 13
p.resetJointState(self.pandaId, i, angles[i])
def get_arm_joints(self): # use list
info = p.getJointStates(self.pandaId, [0, 1, 2, 3, 4, 5, 6])
angles = [x[0] for x in info]
return angles
def get_gripper_joint(self):
info = p.getJointState(self.pandaId, self.pandaLeftFingerJointIndex)
return info[0], info[1]
def get_ee(self):
info = p.getLinkState(self.pandaId, self.pandaEndEffectorLinkIndex)
return array(info[4]), array(info[5])
def get_left_finger(self):
info = p.getLinkState(self.pandaId, self.pandaLeftFingerLinkIndex)
return array(info[4]), array(info[5])
def get_right_finger(self):
info = p.getLinkState(self.pandaId, self.pandaRightFingerLinkIndex)
return array(info[4]), array(info[5])
def get_obs(self):
observation = []
# ee
state = p.getLinkState(self.pandaId,
self.pandaEndEffectorLinkIndex,
computeLinkVelocity=1)
observation += [[
array(state[0]),
array(state[1]),
array(state[6]),
array(state[7])
]] # pos/orn/velL/velA
# joints (arm and fingers) pos and vel
jointStates = p.getJointStates(self.pandaId,
[0, 1, 2, 3, 4, 5, 6, 9, 11])
jointPoses = [[x[0], x[1]] for x in jointStates]
observation += [jointPoses]
return observation