robot_hand.py
Go to the documentation of this file.
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
32  self.radius = radius
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 
48  assert(self.isCapsule() and c2.isCapsule())
49  l1 = self.length
50  r1 = self.radius
51  l2 = c2.length
52  r2 = c2.radius
53 
54  a1 = M1.act(np.matrix([0, 0, -l1 / 2]).T)
55  b1 = M2.act(np.matrix([0, 0, -l2 / 2]).T)
56  a2 = M1.act(np.matrix([0, 0, +l1 / 2]).T)
57  b2 = M2.act(np.matrix([0, 0, +l2 / 2]).T)
58 
59  ab = pinv(np.hstack([a1 - a2, b2 - b1])) * (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) * (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) * (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.matrix([1., 0, 0]).T
100  r1 = cross(ab, x)
101  if norm(r1) < 1e-2:
102  x = np.matrix([0, 1., 0]).T
103  r1 = cross(ab, x)
104  r1 /= norm(r1)
105  r2 = cross(ab, r1)
106  R = np.hstack([r1, r2, ab])
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.matrix([x, y, z]).T)
162 
163  def inertia(m, c):
164  return pin.Inertia(m, np.matrix(c, np.double).T, 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
181  self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
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 
185  self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
186  self.visuals.append(Visual(body_name('wpalmb'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
187 
188  self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
189  pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
190  self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
191 
192  self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
193  pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
194  self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
195 
196  self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
197  pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
198  self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
199 
200  joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
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.matrix([2 * cm, W / 2, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
214  self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
215 
216  joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
222  self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
223 
224  joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
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())
227  self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
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.matrix([2 * cm, 0, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
235  self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
236 
237  joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
242  self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
243 
244  joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
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())
247  self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
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.matrix([2 * cm, -W / 2, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
255  self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
256 
257  joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
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.matrix([FL / 2 - H, 0, 0]).T)
262  self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
263 
264  joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
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())
267  self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
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.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
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.matrix([1 * cm, -1 * cm, 0]).T)
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.matrix([3 * cm, -1.8 * cm, 0]).T)
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.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
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()
def jacobian(self, c2, robot, q)
Definition: robot_hand.py:117
def isCapsule(self)
Definition: robot_hand.py:38
def collisionJacobian(self, pairIndex, q)
Definition: robot_hand.py:298
Eigen::Matrix3d rotate(const std::string &axis, const double ang)
Definition: expose-rpy.cpp:19
def createHand(self, root_id=0, prefix='', joint_placement=None)
Definition: robot_hand.py:159
def place(self, display, oMjoint)
Definition: robot_hand.py:34
def display(self, q)
Definition: robot_hand.py:313
def displayCollision(self, viewer, name='world/wa')
Definition: robot_hand.py:127
def displayCollision(self, pairIndex, meshIndex, onlyOne=False)
Definition: robot_hand.py:304
def __init__(self)
Definition: robot_hand.py:148
def __init__(self, name, jointParent, placement, radius=.1, length=None)
Definition: robot_hand.py:25
def checkCollision(self, pairIndex)
Definition: robot_hand.py:291
def collision(self, c2, data=None, oMj1=None, oMj2=None)
Definition: robot_hand.py:41


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04