def test_add_joint(self):
j1 = pin.JointModelRX()
self.assertTrue(j1.nq == 1)
j2 = pin.JointModelRY()
self.assertTrue(j2.nq == 1)
j3 = pin.JointModelRZ()
self.assertTrue(j3.nq == 1)
jc = pin.JointModelComposite(2)
self.assertTrue(jc.nq == 0)
self.assertTrue(len(jc.joints) == 0)
self.assertTrue(jc.njoints == len(jc.joints))
jc.addJoint(j1)
self.assertTrue(jc.nq == 1)
self.assertTrue(len(jc.joints) == 1)
self.assertTrue(jc.njoints == len(jc.joints))
jc.addJoint(j2)
self.assertTrue(jc.nq == 2)
self.assertTrue(len(jc.joints) == 2)
self.assertTrue(jc.njoints == len(jc.joints))
jc.addJoint(j3, pin.SE3.Random())
self.assertTrue(jc.nq == 3)
self.assertTrue(jc.njoints == len(jc.joints))
def test_add_joint_return(self):
jc1 = pin.JointModelComposite()
jc2 = jc1.addJoint(pin.JointModelRX())
jc3 = jc2.addJoint(pin.JointModelRY())
jc4 = jc1.addJoint(pin.JointModelRZ())
self.assertTrue(jc1.njoints == 3)
self.assertTrue(jc2.njoints == 3)
self.assertTrue(jc3.njoints == 3)
self.assertTrue(jc4.njoints == 3)
del jc1
del jc3
del jc4
self.assertTrue(jc2.njoints == 3)
def __OpenSimJointsToPynocchioJoints(self, PyModel):
jts = 0
for joints in PyModel['Joints']:
dof_in_joint = 6 - (joints[2]['coordinates']).count(None)
if dof_in_joint == 6:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelFreeFlyer()
])
elif dof_in_joint == 3:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelSpherical()
])
elif dof_in_joint == 2:
print '2 dof not supported'
elif dof_in_joint == 1:
for dof in range(0, len(joints[2]['coordinates'])):
if joints[2]['coordinates'][dof] != None:
if joints[2]['name'][dof][0:8] == 'rotation':
if joints[2]['axis'][dof] == ['1', '0', '0']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRY()
]) #Y
elif joints[2]['axis'][dof] == ['0', '1', '0']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRZ()
]) #Z
elif joints[2]['axis'][dof] == ['0', '0', '1']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRX()
]) #X
else:
v = np.matrix([
np.float64(joints[2]['axis'][dof][0]),
np.float64(joints[2]['axis'][dof][1]),
np.float64(joints[2]['axis'][dof][2])
])
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRevoluteUnaligned(
v[0, 2], v[0, 0], v[0, 1])
]) #2,0,1
jts += 1
return self.joint_models
def createHand(self, root_id=0, prefix='', joint_placement=None):
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m**2)
def joint_name(body):
return prefix + body + '_joint'
def body_name(body):
return 'world/' + prefix + body
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
joint_id = root_id
cm = 1e-2
joint_placement = joint_placement if joint_placement is not None else pin.SE3.Identity(
)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('wrist'))
self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]),
pin.SE3.Identity())
L, W, H = 3 * cm, 5 * cm, 1 * cm
self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H,
color)
self.visuals.append(
Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
self.visuals.append(
Visual(body_name('wpalmb'), joint_id,
pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('palm'))
self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
self.visuals.append(
Visual(body_name('palm2'), joint_id,
pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
FL = 4 * cm
palmIdx = joint_id
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger11'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger12'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger13'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
self.visuals.append(
Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger21'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger22'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger23'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
self.visuals.append(
Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(),
joint_placement, joint_name('finger31'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger32'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(
Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(),
joint_placement, joint_name('finger33'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
self.visuals.append(
Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3),
np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement,
joint_name('thumb1'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm,
color)
pos = pin.SE3(
rotate('z', pi / 3) * rotate('x', pi / 2),
np.matrix([1 * cm, -1 * cm, 0]).T)
self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
joint_placement = pin.SE3(
rotate('z', pi / 3) * rotate('x', pi),
np.matrix([3 * cm, -1.8 * cm, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(),
joint_placement, joint_name('thumb2'))
self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]),
pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H,
color)
pos = pin.SE3(rotate('x', pi / 3),
np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
self.visuals.append(
Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
# Prepare some patches to represent collision points. Yet unvisible.
for i in range(10):
self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003,
[1.0, 0, 0, 1])
self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003,
[1.0, 0, 0, 1])
self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
def createLeg6DOF(self, rootId=0, prefix='', jointPlacement=None):
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
jointId = rootId
sphereSize = .15
name = prefix + "hip1"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = jointPlacement if jointPlacement != None else se3.SE3.Identity(
)
jointId = self.model.addJoint(jointId, se3.JointModelRX(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
name = prefix + "hip2"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3.Identity()
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
name = prefix + "hip3"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3.Identity()
jointId = self.model.addJoint(jointId, se3.JointModelRZ(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'hip', sphereSize,
colorred)
self.visuals.append(
Visual('world/' + prefix + 'hip', jointId,
se3.SE3.Identity())) #3 => jointId
self.viewer.viewer.gui.addBox('world/' + prefix + 'thigh', .1, .1, .5,
color)
self.visuals.append(
Visual('world/' + prefix + 'thigh', jointId,
se3.SE3(eye(3), np.matrix([0., 0., -.5]))))
name = prefix + "knee"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([0, 0, -1.0]))
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'knee',
sphereSize, colorred)
self.visuals.append(
Visual('world/' + prefix + 'knee', jointId, se3.SE3.Identity()))
self.viewer.viewer.gui.addBox('world/' + prefix + 'calf', .1, .1, .5,
color)
self.visuals.append(
Visual('world/' + prefix + 'calf', jointId,
se3.SE3(eye(3), np.matrix([0., 0., -.5]))))
name = prefix + "hankle1"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([0, 0, -1.0]))
jointId = self.model.addJoint(jointId, se3.JointModelRX(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
name = prefix + "hankle2"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3.Identity()
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, se3.Inertia.Random(),
se3.SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'hankle',
sphereSize, colorred)
self.visuals.append(
Visual('world/' + prefix + 'hankle', jointId, se3.SE3.Identity()))
self.viewer.viewer.gui.addBox('world/' + prefix + 'foot', .5, .1, .1,
color)
self.visuals.append(
Visual('world/' + prefix + 'foot', jointId,
se3.SE3(eye(3), np.matrix([-.3, 0., 0.]))))
def createHand(self, rootId=0, prefix='', jointPlacement=None):
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
jointId = rootId
cm = 1e-2
trans = lambda x, y, z: se3.SE3(eye(3), np.matrix([x, y, z]).T)
inertia = lambda m, c: se3.Inertia(m,
np.matrix(c, np.double).T,
eye(3) * m**2)
name = prefix + "wrist"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = jointPlacement if jointPlacement != None else se3.SE3.Identity(
)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(3, [0, 0, 0]),
se3.SE3.Identity())
L = 3 * cm
W = 5 * cm
H = 1 * cm
try:
self.viewer.viewer.gui.addSphere('world/' + prefix + 'wrist', .02,
color)
except:
pass
try:
self.viewer.viewer.gui.addBox('world/' + prefix + 'wpalm', L / 2,
W / 2, H, color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'wpalm', jointId, trans(L / 2, 0, 0)))
capsr = H
capsl = W
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'wpalmb',
capsr, capsl, color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'wpalmb', jointId,
se3.SE3(rotate('x', pi / 2), zero(3)), capsr, capsl))
capsr = H
capsl = W
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'wpalmt',
capsr, capsl, color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'wpalmt', jointId,
se3.SE3(rotate('x', pi / 2),
np.matrix([L, 0, 0]).T), capsr, capsl))
capsr = H
capsl = L
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'wpalml', H,
L, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'wpalml', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([L / 2, -W / 2, 0]).T), capsr, capsl))
capsr = H
capsl = L
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'wpalmr', H,
L, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'wpalmr', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([L / 2, W / 2, 0]).T), capsr, capsl))
name = prefix + "palm"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(2, [0, 0, 0]),
se3.SE3.Identity())
capsr = 1 * cm
capsl = W
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'palm2',
1 * cm, W, color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'palm2', jointId,
se3.SE3(rotate('x', pi / 2), zero(3)), capsr, capsl))
FL = 4 * cm
palmIdx = jointId
name = prefix + "finger11"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
jointId = self.model.addJoint(palmIdx, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger11',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger11', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger12"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger12',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger12', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger13"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.3, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = 0.
try:
self.viewer.viewer.gui.addSphere('world/' + prefix + 'finger13',
capsr, color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'finger13', jointId, trans(2 * H, 0, 0),
capsr, capsl))
name = prefix + "finger21"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
jointId = self.model.addJoint(palmIdx, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger21',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger21', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger22"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger22',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger22', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger23"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.3, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = 0.
try:
self.viewer.viewer.gui.addSphere('world/' + prefix + 'finger23', H,
color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'finger23', jointId, trans(H, 0, 0),
capsr, capsl))
name = prefix + "finger31"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
jointId = self.model.addJoint(palmIdx, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger31',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger31', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger32"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'finger32',
H, FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'finger32', jointId,
se3.SE3(rotate('y', pi / 2),
np.matrix([FL / 2 - H, 0, 0]).T), capsr, capsl))
name = prefix + "finger33"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.3, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = 0.
try:
self.viewer.viewer.gui.addSphere('world/' + prefix + 'finger33', H,
color)
except:
pass
self.visuals.append(
Visual('world/' + prefix + 'finger33', jointId, trans(2 * H, 0, 0),
capsr, capsl))
name = prefix + "thumb1"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(eye(3),
np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
jointId = self.model.addJoint(1, se3.JointModelRY(), jointPlacement,
jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = 2 * cm
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'thumb1', H,
2 * cm, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'thumb1', jointId,
se3.SE3(
rotate('z', pi / 3) * rotate('x', pi / 2),
np.matrix([1 * cm, -1 * cm, 0]).T), capsr, capsl))
name = prefix + "thumb2"
jointName, bodyName = [name + "_joint", name + "_body"]
jointPlacement = se3.SE3(
rotate('z', pi / 3) * rotate('x', pi),
np.matrix([3 * cm, -1.8 * cm, 0]).T)
jointId = self.model.addJoint(jointId, se3.JointModelRZ(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.4, [0, 0, 0]),
se3.SE3.Identity())
capsr = H
capsl = FL - 2 * H
try:
self.viewer.viewer.gui.addCapsule('world/' + prefix + 'thumb2', H,
FL - 2 * H, color)
except:
pass
self.visuals.append(
Visual(
'world/' + prefix + 'thumb2', jointId,
se3.SE3(rotate('x', pi / 3),
np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T), capsr,
capsl))
# Prepare some patches to represent collision points. Yet unvisible.
for i in range(10):
try:
self.viewer.viewer.gui.addCylinder('world/wa' + str(i), .01,
.003, [1.0, 0, 0, 1])
self.viewer.viewer.gui.addCylinder('world/wb' + str(i), .01,
.003, [1.0, 0, 0, 1])
except:
pass
self.viewer.viewer.gui.setVisibility('world/wa' + str(i), 'OFF')
self.viewer.viewer.gui.setVisibility('world/wb' + str(i), 'OFF')
def _parse2PinocchioJoints(pymodel):
jts = 0
# save pinocchio like joint models
joint_models = []
# axis transformations (e.g. for inversing sign with left and right)
joint_transformations = []
for joints in pymodel['Joints']:
dof_in_joint = 6 - (joints[2]['coordinates']).count(None)
if dof_in_joint == 6:
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis'])))
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelFreeFlyer()
])
elif dof_in_joint == 3:
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis']))[0:3, :])
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelSpherical()
])
elif dof_in_joint == 2:
print '2 dof not supported'
elif dof_in_joint == 1:
for dof in range(0, len(joints[2]['coordinates'])):
if joints[2]['coordinates'][dof] != None:
if joints[2]['name'][dof][0:8] == 'rotation':
if joints[2]['axis'][dof] == [
'1', '0', '0'
] or joints[2]['axis'][dof] == ['-1', '0', '0']:
#Y
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelRY()
])
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis']))[dof])
elif joints[2]['axis'][dof] == ['0', '1', '0']:
#Z
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelRZ()
])
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis']))[dof])
elif joints[2]['axis'][dof] == ['0', '0', '1']:
#X
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelRX()
])
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis']))[dof])
else:
joint_transformations.append(
np.matrix(np.float64(joints[2]['axis']))[dof])
v = np.matrix([
np.float64(joints[2]['axis'][dof][0]),
np.float64(joints[2]['axis'][dof][1]),
np.float64(joints[2]['axis'][dof][2])
])
#2,0,1
joint_models.append([
jts, pymodel['Joints'][jts][0]['name'][0],
se3.JointModelRevoluteUnaligned(
v[0, 2], v[0, 0], v[0, 1])
])
jts += 1
return joint_models, joint_transformations
def createHand(self,rootId=0,jointPlacement=None):
color = [red,green,blue,transparency] = [1,1,0.78,1.0]
colorred = [1.0,0.0,0.0,1.0]
jointId = rootId
cm = 1e-2
trans = lambda x,y,z: pio.SE3(eye(3),np.matrix([x,y,z]).T)
inertia = lambda m,c: pio.Inertia(m,np.matrix(c,np.double).T,eye(3)*m**2)
name = "wrist"
jointName,bodyName = [name+"_joint",name+"_body"]
#jointPlacement = jointPlacement if jointPlacement!=None else pio.SE3.Identity()
jointPlacement = jointPlacement if jointPlacement!=None else pio.SE3(pio.utils.rotate('y',np.pi),zero(3))
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(3,[0,0,0]),pio.SE3.Identity())
## Hand dimsensions: length, width, height(=depth), finger-length
L=3*cm;W=5*cm;H=1*cm; FL = 4*cm
self.addCapsule('world/wrist',jointId,
pio.SE3(rotate('x',pi/2),np.matrix([0,0,0]).T),.02,0 )
self.addCapsule('world/wpalml',jointId,
pio.SE3(rotate('z',-.3)*rotate('y',pi/2),np.matrix([L/2,-W/2.6,0]).T),H,L )
#pio.SE3(rotate('y',pi/2),np.matrix([L/2,-W/2,0]).T),H,L )
self.addCapsule('world/wpalmr',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([L/2,W/2,0]).T),H,L)
self.addCapsule('world/wpalmfr',jointId,
pio.SE3(rotate('x',pi/2),np.matrix([L,0,0]).T),H,W)
name = "palm"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([5*cm,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(2,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/palm2',jointId,
# pio.SE3(rotate('y',pi/2),zero(3)),1*cm,W )
pio.SE3(rotate('x',pi/2),zero(3)),1*cm,W )
palmIdx = jointId
name = "finger11"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,W/2,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger11',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger12"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger12',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger13"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-2*H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger13',jointId,
trans(2*H,0,0),H,0 )
name = "finger21"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,0,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger21',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger22"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger22',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger23"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger23',jointId,
trans(H,0,0),H,0 )
name = "finger31"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,-W/2,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger31',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H)
name = "finger32"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger32',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H)
name = "finger33"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-2*H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger33',jointId,
trans(2*H,0,0),H,0)
name = "thumb1"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(rotate('z',-1), np.matrix([1*cm,-W/2-H*1.3,0]).T)
jointId = self.model.addJoint(1,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
# self.addCapsule('world/thumb1',jointId,
# pio.SE3(rotate('z',pi/3)*rotate('x',pi/2),np.matrix([1*cm,-1*cm,0]).T),
# H,2*cm)
name = "thumb1a"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), zero(3))
jointId = self.model.addJoint(jointId,pio.JointModelRX(),jointPlacement,jointName)
# self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/thumb1',jointId,
pio.SE3(rotate('z',pi/3)*rotate('x',pi/2),np.matrix([0.3*cm,-1.0*cm,0]).T),
H,2*cm)
name = "thumb2"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(rotate('z',pi/3)*rotate('x',pi), np.matrix([3*cm,-1.8*cm,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRZ(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.4,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/thumb2',jointId,
pio.SE3(rotate('x',pi/3),np.matrix([-0.7*cm,.8*cm,-0.5*cm]).T),
H,FL-2*H)
# Prepare some patches to represent collision points. Yet unvisible.
if self.viewer is not None:
self.maxContact = 10
for i in range(self.maxContact):
self.viewer.addCylinder('world/cpatch%d'%i, .01, .003, [ 1.0,0,0,1])
self.viewer.setVisibility('world/cpatch%d'%i,'OFF')