4 import pinocchio
as pin
5 from numpy.linalg
import norm, pinv
6 from pinocchio.utils
import cross, eye, rotate, zero
8 from display
import Display
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).
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.
26 def __init__(self, name, jointParent, placement, radius=0.1, length=None):
27 """Length and radius are used in case of capsule objects"""
31 if length
is not None:
35 def place(self, display, oMjoint):
36 oMbody = oMjoint * self.
placementplacement
37 display.place(self.
namename, oMbody,
False)
40 return hasattr(self,
"length")
and hasattr(self,
"radius")
42 def collision(self, c2, data=None, oMj1=None, oMj2=None):
45 oMj2 = data.oMi[c2.jointParent]
47 M2 = oMj2 * c2.placement
48 assert self.
isCapsuleisCapsule()
and c2.isCapsule()
54 a1 = M1.act(np.array([0, 0, -l1 / 2]))
55 b1 = M2.act(np.array([0, 0, -l2 / 2]))
56 a2 = M1.act(np.array([0, 0, +l1 / 2]))
57 b2 = M2.act(np.array([0, 0, +l2 / 2]))
59 a1a2_b1b2 = np.array([a1 - a2, b1 - b2]).T
60 ab = pinv(a1a2_b1b2) * (b2 - a2)
62 if (0 <= ab).all()
and (ab <= 1).all():
64 pa = a2 + ab[0, 0] * (a1 - a2)
65 pb = b2 + ab[1, 0] * (b1 - b2)
68 i = np.argmin(np.vstack([ab, 1 - ab]))
70 pa = a2
if i == 0
else a1
71 pb = b2
if i == 1
else b1
73 b = (pinv((b1 - b2).reshape(3, 1)) * (pa - b2))[0, 0]
79 pb = b2 + b * (b1 - b2)
82 a = (pinv((a1 - a2).reshape(3, 1)) * (pb - a2))[0, 0]
88 pa = a2 + a * (a1 - a2)
92 dist = norm(pa - pb) - (r1 + r2)
93 if norm(pa - pb) > 1e-3:
101 x = np.array([1.0, 0, 0])
104 x = np.array([0, 1.0, 0])
108 R = np.hstack([r1, r2, ab.reshape(3, 1)])
119 def jacobian(self, c2, robot, q):
120 Ja = pin.jacobian(robot.model, robot.data, q, self.
jointParentjointParent,
False,
True)
121 Jb = pin.jacobian(robot.model, robot.data, q, c2.jointParent,
False,
True)
123 Xa = pin.SE3(self.
RR, self.
ww).action
124 Xb = pin.SE3(c2.R, c2.w).action
126 J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
129 def displayCollision(self, viewer, name="world/wa"):
130 viewer.viewer.gui.setVisibility(name,
"ON")
131 viewer.place(name, pin.SE3(self.
RR, self.
ww))
136 Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
137 The configuration is nq=7. The velocity is the same.
138 The members of the class are:
139 * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
140 * model: the kinematic tree of the robot.
141 * data: the temporary variables to be used by the kinematic algorithms.
142 * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
143 an object Visual (see above).
145 CollisionPairs is a list of visual indexes.
146 Reference to the collision pair is used in the collision test and jacobian of the collision
147 (which are simply proxy method to methods of the visual class).
153 self.
modelmodel = pin.Model()
155 self.
datadata = self.
modelmodel.createData()
156 self.
q0q0 = zero(self.
modelmodel.nq)
158 self.
v0v0 = zero(self.
modelmodel.nv)
161 def createHand(self, root_id=0, prefix="", joint_placement=None):
163 return pin.SE3(eye(3), np.array([x, y, z]))
166 return pin.Inertia(m, np.array(c, np.double), eye(3) * m**2)
168 def joint_name(body):
169 return prefix + body +
"_joint"
172 return "world/" + prefix + body
174 color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
179 joint_placement
if joint_placement
is not None else pin.SE3.Identity()
181 joint_id = self.
modelmodel.addJoint(
182 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"wrist")
184 self.
modelmodel.appendBodyToJoint(
185 joint_id, inertia(3, [0, 0, 0]), pin.SE3.Identity()
188 L, W, H = 3 * cm, 5 * cm, 1 * cm
189 self.
viewerviewer.viewer.gui.addSphere(body_name(
"wrist"), 0.02, color)
190 self.
viewerviewer.viewer.gui.addBox(body_name(
"wpalm"), L / 2, W / 2, H, color)
191 self.
visualsvisuals.append(
Visual(body_name(
"wpalm"), joint_id, trans(L / 2, 0, 0)))
193 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"wpalmb"), H, W, color)
198 pin.SE3(rotate(
"x", pi / 2), zero(3)),
204 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"wpalmt"), H, W, color)
205 pos = pin.SE3(rotate(
"x", pi / 2), np.array([L, 0, 0]))
206 self.
visualsvisuals.append(
Visual(body_name(
"wpalmt"), joint_id, pos, H, W))
208 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"wpalml"), H, L, color)
209 pos = pin.SE3(rotate(
"y", pi / 2), np.array([L / 2, -W / 2, 0]))
210 self.
visualsvisuals.append(
Visual(body_name(
"wpalml"), joint_id, pos, H, L))
212 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"wpalmr"), H, L, color)
213 pos = pin.SE3(rotate(
"y", pi / 2), np.array([L / 2, +W / 2, 0]))
214 self.
visualsvisuals.append(
Visual(body_name(
"wpalmr"), joint_id, pos, H, L))
216 joint_placement = pin.SE3(eye(3), np.array([5 * cm, 0, 0]))
217 joint_id = self.
modelmodel.addJoint(
218 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"palm")
220 self.
modelmodel.appendBodyToJoint(
221 joint_id, inertia(2, [0, 0, 0]), pin.SE3.Identity()
223 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"palm2"), 1 * cm, W, color)
228 pin.SE3(rotate(
"x", pi / 2), zero(3)),
237 joint_placement = pin.SE3(eye(3), np.array([2 * cm, W / 2, 0]))
238 joint_id = self.
modelmodel.addJoint(
239 palmIdx, pin.JointModelRY(), joint_placement, joint_name(
"finger11")
241 self.
modelmodel.appendBodyToJoint(
242 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
244 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger11"), H, FL - 2 * H, color)
245 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
246 self.
visualsvisuals.append(
Visual(body_name(
"finger11"), joint_id, pos, H, FL - 2 * H))
248 joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
249 joint_id = self.
modelmodel.addJoint(
250 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger12")
252 self.
modelmodel.appendBodyToJoint(
253 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
256 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger12"), H, FL - 2 * H, color)
257 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
258 self.
visualsvisuals.append(
Visual(body_name(
"finger12"), joint_id, pos, H, FL - 2 * H))
260 joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
261 joint_id = self.
modelmodel.addJoint(
262 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger13")
264 self.
modelmodel.appendBodyToJoint(
265 joint_id, inertia(0.3, [0, 0, 0]), pin.SE3.Identity()
267 self.
viewerviewer.viewer.gui.addSphere(body_name(
"finger13"), H, color)
269 Visual(body_name(
"finger13"), joint_id, trans(2 * H, 0, 0), H, 0)
272 joint_placement = pin.SE3(eye(3), np.array([2 * cm, 0, 0]))
273 joint_id = self.
modelmodel.addJoint(
274 palmIdx, pin.JointModelRY(), joint_placement, joint_name(
"finger21")
276 self.
modelmodel.appendBodyToJoint(
277 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
279 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger21"), H, FL - 2 * H, color)
280 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
281 self.
visualsvisuals.append(
Visual(body_name(
"finger21"), joint_id, pos, H, FL - 2 * H))
283 joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
284 joint_id = self.
modelmodel.addJoint(
285 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger22")
287 self.
modelmodel.appendBodyToJoint(
288 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
290 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger22"), H, FL - 2 * H, color)
291 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
292 self.
visualsvisuals.append(
Visual(body_name(
"finger22"), joint_id, pos, H, FL - 2 * H))
294 joint_placement = pin.SE3(eye(3), np.array([FL - H, 0, 0]))
295 joint_id = self.
modelmodel.addJoint(
296 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger23")
298 self.
modelmodel.appendBodyToJoint(
299 joint_id, inertia(0.3, [0, 0, 0]), pin.SE3.Identity()
301 self.
viewerviewer.viewer.gui.addSphere(body_name(
"finger23"), H, color)
303 Visual(body_name(
"finger23"), joint_id, trans(H, 0, 0), H, 0)
306 joint_placement = pin.SE3(eye(3), np.array([2 * cm, -W / 2, 0]))
307 joint_id = self.
modelmodel.addJoint(
308 palmIdx, pin.JointModelRY(), joint_placement, joint_name(
"finger31")
310 self.
modelmodel.appendBodyToJoint(
311 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
313 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger31"), H, FL - 2 * H, color)
314 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
315 self.
visualsvisuals.append(
Visual(body_name(
"finger31"), joint_id, pos, H, FL - 2 * H))
317 joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
318 joint_id = self.
modelmodel.addJoint(
319 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger32")
321 self.
modelmodel.appendBodyToJoint(
322 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
324 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"finger32"), H, FL - 2 * H, color)
325 pos = pin.SE3(rotate(
"y", pi / 2), np.array([FL / 2 - H, 0, 0]))
326 self.
visualsvisuals.append(
Visual(body_name(
"finger32"), joint_id, pos, H, FL - 2 * H))
328 joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
329 joint_id = self.
modelmodel.addJoint(
330 joint_id, pin.JointModelRY(), joint_placement, joint_name(
"finger33")
332 self.
modelmodel.appendBodyToJoint(
333 joint_id, inertia(0.3, [0, 0, 0]), pin.SE3.Identity()
335 self.
viewerviewer.viewer.gui.addSphere(body_name(
"finger33"), H, color)
337 Visual(body_name(
"finger33"), joint_id, trans(2 * H, 0, 0), H, 0)
340 joint_placement = pin.SE3(eye(3), np.array([1 * cm, -W / 2 - H * 1.5, 0]))
341 joint_id = self.
modelmodel.addJoint(
342 1, pin.JointModelRY(), joint_placement, joint_name(
"thumb1")
344 self.
modelmodel.appendBodyToJoint(
345 joint_id, inertia(0.5, [0, 0, 0]), pin.SE3.Identity()
347 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"thumb1"), H, 2 * cm, color)
349 rotate(
"z", pi / 3) * rotate(
"x", pi / 2), np.array([1 * cm, -1 * cm, 0])
351 self.
visualsvisuals.append(
Visual(body_name(
"thumb1"), joint_id, pos, 2 * cm))
353 joint_placement = pin.SE3(
354 rotate(
"z", pi / 3) * rotate(
"x", pi), np.array([3 * cm, -1.8 * cm, 0])
356 joint_id = self.
modelmodel.addJoint(
357 joint_id, pin.JointModelRZ(), joint_placement, joint_name(
"thumb2")
359 self.
modelmodel.appendBodyToJoint(
360 joint_id, inertia(0.4, [0, 0, 0]), pin.SE3.Identity()
362 self.
viewerviewer.viewer.gui.addCapsule(body_name(
"thumb2"), H, FL - 2 * H, color)
363 pos = pin.SE3(rotate(
"x", pi / 3), np.array([-0.7 * cm, 0.8 * cm, -0.5 * cm]))
364 self.
visualsvisuals.append(
Visual(body_name(
"thumb2"), joint_id, pos, H, FL - 2 * H))
368 self.
viewerviewer.viewer.gui.addCylinder(
369 "world/wa%i" % i, 0.01, 0.003, [1.0, 0, 0, 1]
371 self.
viewerviewer.viewer.gui.addCylinder(
372 "world/wb%i" % i, 0.01, 0.003, [1.0, 0, 0, 1]
374 self.
viewerviewer.viewer.gui.setVisibility(
"world/wa%i" % i,
"OFF")
375 self.
viewerviewer.viewer.gui.setVisibility(
"world/wb%i" % i,
"OFF")
377 def checkCollision(self, pairIndex):
381 dist = va.collision(vb, self.
datadata)
384 def collisionJacobian(self, pairIndex, q):
388 return va.jacobian(vb, self, q)
390 def displayCollision(self, pairIndex, meshIndex, onlyOne=False):
394 va.displayCollision(self.
viewerviewer,
"world/wa%i" % meshIndex)
395 vb.displayCollision(self.
viewerviewer,
"world/wb%i" % meshIndex)
396 self.
viewerviewer.viewer.gui.setVisibility(
"world/wa%i" % meshIndex,
"ON")
397 self.
viewerviewer.viewer.gui.setVisibility(
"world/wb%i" % meshIndex,
"ON")
400 pin.forwardKinematics(self.
modelmodel, self.
datadata, q)
401 for visual
in self.
visualsvisuals:
402 visual.place(self.
viewerviewer, self.
datadata.oMi[visual.jointParent])
403 self.
viewerviewer.viewer.gui.refresh()
def createHand(self, root_id=0, prefix="", joint_placement=None)
def __init__(self, name, jointParent, placement, radius=0.1, length=None)