pinocchio  2.6.21 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
robot_hand.py
1 from math import pi
2
3 import numpy as np
4 from numpy.linalg import norm, pinv
5
6 import pinocchio as pin
7 from pinocchio.utils import cross, zero, rotate, eye
8 from display import Display
9
10
11 class Visual(object):
12  '''
13  Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
14  * the name of the 3D objects inside Gepetto viewer.
15  * the ID of the joint in the kinematic tree to which the body is attached.
16  * the placement of the body with respect to the joint frame.
17  This class is only used in the list Robot.visuals (see below).
18
19  The visual are supposed mostly to be capsules. In that case, the object also contains
20  radius and length of the capsule.
21  The collision checking computes collision test, distance, and witness points.
22  Using the supporting robot, the collision Jacobian returns a 1xN matrix corresponding
23  to the normal direction.
24  '''
25  def __init__(self, name, jointParent, placement, radius=.1, length=None):
26  '''Length and radius are used in case of capsule objects'''
27  self.name = name # Name in gepetto viewer
28  self.jointParent = jointParent # ID (int) of the joint
29  self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint
30  if length is not None:
31  self.length = length
33
34  def place(self, display, oMjoint):
35  oMbody = oMjoint * self.placement
36  display.place(self.name, oMbody, False)
37
38  def isCapsule(self):
39  return hasattr(self, 'length') and hasattr(self, 'radius')
40
41  def collision(self, c2, data=None, oMj1=None, oMj2=None):
42  if data is not None:
43  oMj1 = data.oMi[self.jointParent]
44  oMj2 = data.oMi[c2.jointParent]
45  M1 = oMj1 * self.placement
46  M2 = oMj2 * c2.placement
47  assert (self.isCapsule() and c2.isCapsule())
48  l1 = self.length
50  l2 = c2.length
52
53  a1 = M1.act(np.array([0, 0, -l1 / 2]))
54  b1 = M2.act(np.array([0, 0, -l2 / 2]))
55  a2 = M1.act(np.array([0, 0, +l1 / 2]))
56  b2 = M2.act(np.array([0, 0, +l2 / 2]))
57
58  a1a2_b1b2 = np.array([a1 - a2, b1 - b2]).T
59  ab = pinv(a1a2_b1b2) * (b2 - a2)
60
61  if (0 <= ab).all() and (ab <= 1).all():
62  asat = bsat = False
63  pa = a2 + ab[0, 0] * (a1 - a2)
64  pb = b2 + ab[1, 0] * (b1 - b2)
65  else:
66  asat = bsat = True
67  i = np.argmin(np.vstack([ab, 1 - ab]))
68
69  pa = a2 if i == 0 else a1
70  pb = b2 if i == 1 else b1
71  if i == 0 or i == 2: # fix a to pa, search b
72  b = (pinv((b1 - b2).reshape(3, 1)) * (pa - b2))[0, 0]
73  if b < 0:
74  pb = b2
75  elif b > 1:
76  pb = b1
77  else:
78  pb = b2 + b * (b1 - b2)
79  bsat = False
80  else: # fix b
81  a = (pinv((a1 - a2).reshape(3, 1)) * (pb - a2))[0, 0]
82  if a < 0:
83  pa = a2
84  elif a > 1:
85  pa = a1
86  else:
87  pa = a2 + a * (a1 - a2)
88  asat = False
89
90  dist = norm(pa - pb) - (r1 + r2)
91  if norm(pa - pb) > 1e-3:
92  # Compute witness points
93  ab = pa - pb
94  ab /= norm(ab)
95  wa = pa - ab * r1
96  wb = pb + ab * r2
97
98  # Compute normal matrix
99  x = np.array([1., 0, 0])
100  r1 = cross(ab, x)
101  if norm(r1) < 1e-2:
102  x = np.array([0, 1., 0])
103  r1 = cross(ab, x)
104  r1 /= norm(r1)
105  r2 = cross(ab, r1)
106  R = np.hstack([r1, r2, ab.reshape(3,1)])
107
108  self.dist = dist
109  c2.dist = dist
110  self.w = wa
111  c2.w = wb
112  self.R = R
113  c2.R = R
114
115  return dist
116
117  def jacobian(self, c2, robot, q):
118  Ja = pin.jacobian(robot.model, robot.data, q, self.jointParent, False, True)
119  Jb = pin.jacobian(robot.model, robot.data, q, c2.jointParent, False, True)
120
121  Xa = pin.SE3(self.R, self.w).action
122  Xb = pin.SE3(c2.R, c2.w).action
123
124  J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
125  return J
126
127  def displayCollision(self, viewer, name='world/wa'):
128  viewer.viewer.gui.setVisibility(name, 'ON')
129  viewer.place(name, pin.SE3(self.R, self.w))
130
131
132 class Robot(object):
133  '''
134  Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
135  The configuration is nq=7. The velocity is the same.
136  The members of the class are:
137  * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
138  * model: the kinematic tree of the robot.
139  * data: the temporary variables to be used by the kinematic algorithms.
140  * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
141  an object Visual (see above).
142
143  CollisionPairs is a list of visual indexes.
144  Reference to the collision pair is used in the collision test and jacobian of the collision
145  (which are simply proxy method to methods of the visual class).
146  '''
147
148  def __init__(self):
149  self.viewer = Display()
150  self.visuals = []
151  self.model = pin.Model()
152  self.createHand()
153  self.data = self.model.createData()
154  self.q0 = zero(self.model.nq)
155  # self.q0[3] = 1.0
156  self.v0 = zero(self.model.nv)
157  self.collisionPairs = []
158
159  def createHand(self, root_id=0, prefix='', joint_placement=None):
160  def trans(x, y, z):
161  return pin.SE3(eye(3), np.array([x, y, z]))
162
163  def inertia(m, c):
164  return pin.Inertia(m, np.array(c, np.double), eye(3) * m ** 2)
165
166  def joint_name(body):
167  return prefix + body + '_joint'
168
169  def body_name(body):
170  return 'world/' + prefix + body
171
172  color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
173  joint_id = root_id
174  cm = 1e-2
175
176  joint_placement = joint_placement if joint_placement is not None else pin.SE3.Identity()
177  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('wrist'))
178  self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]), pin.SE3.Identity())
179
180  L, W, H = 3 * cm, 5 * cm, 1 * cm
182  self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H, color)
183  self.visuals.append(Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
184
186  self.visuals.append(Visual(body_name('wpalmb'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
187
189  pos = pin.SE3(rotate('x', pi / 2), np.array([L, 0, 0]))
190  self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
191
193  pos = pin.SE3(rotate('y', pi / 2), np.array([L / 2, -W / 2, 0]))
194  self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
195
197  pos = pin.SE3(rotate('y', pi / 2), np.array([L / 2, +W / 2, 0]))
198  self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
199
200  joint_placement = pin.SE3(eye(3), np.array([5 * cm, 0, 0]))
201  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('palm'))
202  self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]), pin.SE3.Identity())
203  self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
204  self.visuals.append(Visual(body_name('palm2'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
205
206  FL = 4 * cm
207  palmIdx = joint_id
208
209  joint_placement = pin.SE3(eye(3), np.array([2 * cm, W / 2, 0]))
210  joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger11'))
211  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
212  self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H, color)
213  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
214  self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
215
216  joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
217  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger12'))
218  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
219
220  self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H, color)
221  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
222  self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
223
224  joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
225  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger13'))
226  self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
228  self.visuals.append(Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
229
230  joint_placement = pin.SE3(eye(3), np.array([2 * cm, 0, 0]))
231  joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger21'))
232  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
233  self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H, color)
234  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
235  self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
236
237  joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
238  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger22'))
239  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
240  self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H, color)
241  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
242  self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
243
244  joint_placement = pin.SE3(eye(3), np.array([FL - H, 0, 0]))
245  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger23'))
246  self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
248  self.visuals.append(Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
249
250  joint_placement = pin.SE3(eye(3), np.array([2 * cm, -W / 2, 0]))
251  joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger31'))
252  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
253  self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H, color)
254  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
255  self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
256
257  joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
258  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger32'))
259  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
260  self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H, color)
261  pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
262  self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
263
264  joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
265  joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger33'))
266  self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
268  self.visuals.append(Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
269
270  joint_placement = pin.SE3(eye(3), np.array([1 * cm, -W / 2 - H * 1.5, 0]))
271  joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement, joint_name('thumb1'))
272  self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
273  self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm, color)
274  pos = pin.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.array([1 * cm, -1 * cm, 0]))
275  self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
276
277  joint_placement = pin.SE3(rotate('z', pi / 3) * rotate('x', pi), np.array([3 * cm, -1.8 * cm, 0]))
278  joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(), joint_placement, joint_name('thumb2'))
279  self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]), pin.SE3.Identity())
280  self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H, color)
281  pos = pin.SE3(rotate('x', pi / 3), np.array([-0.7 * cm, .8 * cm, -0.5 * cm]))
282  self.visuals.append(Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
283
284  # Prepare some patches to represent collision points. Yet unvisible.
285  for i in range(10):
286  self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1.0, 0, 0, 1])
287  self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1.0, 0, 0, 1])
288  self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
289  self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
290
291  def checkCollision(self, pairIndex):
292  ia, ib = self.collisionPairs[pairIndex]
293  va = self.visuals[ia]
294  vb = self.visuals[ib]
295  dist = va.collision(vb, self.data)
296  return dist
297
298  def collisionJacobian(self, pairIndex, q):
299  ia, ib = self.collisionPairs[pairIndex]
300  va = self.visuals[ia]
301  vb = self.visuals[ib]
302  return va.jacobian(vb, self, q)
303
304  def displayCollision(self, pairIndex, meshIndex, onlyOne=False):
305  ia, ib = self.collisionPairs[pairIndex]
306  va = self.visuals[ia]
307  vb = self.visuals[ib]
308  va.displayCollision(self.viewer, 'world/wa%i' % meshIndex)
309  vb.displayCollision(self.viewer, 'world/wb%i' % meshIndex)
310  self.viewer.viewer.gui.setVisibility('world/wa%i' % meshIndex, 'ON')
311  self.viewer.viewer.gui.setVisibility('world/wb%i' % meshIndex, 'ON')
312
313  def display(self, q):
314  pin.forwardKinematics(self.model, self.data, q)
315  for visual in self.visuals:
316  visual.place(self.viewer, self.data.oMi[visual.jointParent])
317  self.viewer.viewer.gui.refresh()
robot_hand.Visual.w
w
Definition: robot_hand.py:110
display.Display
Definition: display.py:10
robot_hand.Robot.collisionPairs
collisionPairs
Definition: robot_hand.py:157
robot_hand.Visual
Definition: robot_hand.py:11
robot_hand.Robot.viewer
viewer
Definition: robot_hand.py:149
robot_hand.Robot.createHand
def createHand(self, root_id=0, prefix='', joint_placement=None)
Definition: robot_hand.py:159
robot_hand.Visual.placement
placement
Definition: robot_hand.py:29
Definition: robot_hand.py:32
robot_hand.Visual.R
R
Definition: robot_hand.py:112
robot_hand.Robot
Definition: robot_hand.py:132
robot_hand.Visual.length
length
Definition: robot_hand.py:31
robot_hand.Visual.name
name
Definition: robot_hand.py:27
robot_hand.Robot.v0
v0
Definition: robot_hand.py:156
utils
robot_hand.Robot.data
data
Definition: robot_hand.py:153
robot_hand.Visual.jointParent
jointParent
Definition: robot_hand.py:28
display
Definition: display.py:1
robot_hand.Robot.model
model
Definition: robot_hand.py:151
robot_hand.Robot.visuals
visuals
Definition: robot_hand.py:150
robot_hand.Visual.isCapsule
def isCapsule(self)
Definition: robot_hand.py:38
robot_hand.Robot.q0
q0
Definition: robot_hand.py:154
robot_hand.Visual.__init__
def __init__(self, name, jointParent, placement, radius=.1, length=None)
Definition: robot_hand.py:25
robot_hand.Visual.dist
dist
Definition: robot_hand.py:108