def test_add_joint(self):
model = pin.Model()
idx = 0
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1))
MAX_EFF = 100.
MAX_VEL = 10.
MIN_POS = -1.
MAX_POS = 1.
me = np.array([MAX_EFF])
mv = np.array([MAX_VEL])
lb = np.array([MIN_POS])
ub = np.array([MAX_POS])
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1), me, mv, lb, ub)
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.njoints, 3)
self.assertEqual(model.nq, 2)
self.assertEqual(model.nv, 2)
self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
def createPendulum(self,nbJoint,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
jointPlacement = jointPlacement if jointPlacement!=None else se3.SE3.Identity()
length = self.length
mass = self.mass
# inertia = se3.Inertia(mass,
# np.matrix([0.0,0.0,length/2]).T,
# mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) )
inertia = se3.Inertia(mass,
np.matrix([0.0,0.0,length/2]).T,
0*mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) )
for i in range(nbJoint):
istr = str(i)
name = prefix+"joint"+istr
jointName,bodyName = [name+"_joint",name+"_body"]
jointId = self.model.addJoint(jointId,se3.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia,se3.SE3.Identity())
if self.viewer is not None:
try:self.viewer.viewer.gui.addSphere('world/'+prefix+'sphere'+istr, length*0.15,colorred)
except: pass
self.visuals.append( Visual('world/'+prefix+'sphere'+istr,jointId,se3.SE3.Identity()) )
try:self.viewer.viewer.gui.addCapsule('world/'+prefix+'arm'+istr, length*.1,.8*length,color)
except:pass
self.visuals.append( Visual('world/'+prefix+'arm'+istr,jointId,
se3.SE3(eye(3),np.matrix([0.,0.,length/2]))))
jointPlacement = se3.SE3(eye(3),np.matrix([0.0,0.0,length]).T)
self.model.addFrame( se3.Frame('tip',jointId,0,jointPlacement,se3.FrameType.OP_FRAME) )
def createMonoped(nbJoint, linkLength=1.0, floatingMass=1.0, linkMass=1.0):
rmodel = pinocchio.Model()
prefix = ''
baseInertia = pinocchio.Inertia(floatingMass,
np.matrix([0.0, 0.0, 0.0]).T,
np.diagflat([1e-6, 1e-6, 1e-6]))
linkInertia = pinocchio.Inertia(linkMass,
np.matrix([0.0, 0.0, linkLength/2]).T,
linkMass/5*np.diagflat([1e-2, linkLength**2, 1e-2]))
# PRISMATIC JOINT
jointId = 0
jointPlacement = pinocchio.SE3.Identity()
jointName,bodyName = ["prismatic_joint", "mass"]
jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName)
rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity())
# REVOLUTE JOINTS
for i in range(1, nbJoint + 1):
jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)]
jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName)
rmodel.appendBodyToJoint(jointId,linkInertia, pinocchio.SE3.Identity())
rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT))
jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T)
rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0)
return rmodel
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 createPendulum(nbJoint, length=1.0, mass=1.0, viewer=None):
'''
Creates the Pinocchio kinematic <rmodel> and visuals <gmodel> models for
a N-pendulum.
@param nbJoint: number of joints <N> of the N-pendulum.
@param length: length of each arm of the pendulum.
@param mass: mass of each arm of the pendulum.
@param viewer: gepetto-viewer CORBA client. If not None, then creates the geometries
in the viewer.
'''
rmodel = pio.Model()
gmodel = pio.GeometryModel()
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
radius = 0.1 * length
prefix = ''
jointId = 0
jointPlacement = pio.SE3.Identity()
inertia = pio.Inertia(mass,
np.matrix([0.0, 0.0, length / 2]).T,
mass / 5 * np.diagflat([1e-2, length**2, 1e-2]))
viewerPrefix = getViewerPrefix(viewer, ["world", "pinocchio", "visuals"])
for i in range(nbJoint):
istr = str(i)
name = prefix + "joint" + istr
jointName, bodyName = [name + "_joint", name + "_body"]
jointId = rmodel.addJoint(jointId, pio.JointModelRY(), jointPlacement,
jointName)
rmodel.appendBodyToJoint(jointId, inertia, pio.SE3.Identity())
if viewer is not None:
viewer.addSphere(viewerPrefix + jointName, 1.5 * radius, colorred)
viewer.addCapsule(viewerPrefix + bodyName, radius, .8 * length,
color)
gmodel.addGeometryObject(
Capsule(jointName, jointId, pio.SE3.Identity(), 1.5 * radius, 0.0))
gmodel.addGeometryObject(
Capsule(bodyName, jointId,
pio.SE3(eye(3),
np.matrix([0., 0., length / 2]).T), radius,
0.8 * length))
jointPlacement = pio.SE3(eye(3), np.matrix([0.0, 0.0, length]).T)
rmodel.addFrame(
pio.Frame('tip', jointId, 0, jointPlacement, pio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.zeros(nbJoint) + 2 * np.pi
rmodel.lowerPositionLimit = np.zeros(nbJoint) - 2 * np.pi
rmodel.velocityLimit = np.zeros(nbJoint) + 5.0
return rmodel, gmodel
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 createSoloTB():
rmodel = pinocchio.Model()
prefix = ''
nbJoint = 2
floatingMass = 1.48538/4 # one quater of total base mass
linkLength = 0.16
firstLinkMass = 0.148538
secondLinkMass = 0.0376361
firstLinkCOMLength = 0.078707
secondLinkCOMLength = 0.102249
baseInertia = pinocchio.Inertia(floatingMass,
np.matrix([0.0, 0.0, 0.0]).T,
np.diagflat([1e-6, 1e-6, 1e-6]))
firstLinkInertia = pinocchio.Inertia(firstLinkMass,
np.matrix([0.0, 0.0, firstLinkCOMLength]).T,
firstLinkMass/3*np.diagflat([1e-6, firstLinkCOMLength**2, 1e-6]))
secondLinkInertia = pinocchio.Inertia(secondLinkMass,
np.matrix([0.0, 0.0, secondLinkCOMLength]).T,
secondLinkMass/3*np.diagflat([1e-6, secondLinkCOMLength**2, 1e-6]))
# PRISMATIC JOINT
jointId = 0
jointPlacement = pinocchio.SE3.Identity()
jointName,bodyName = ["prismatic_joint", "mass"]
jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName)
rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity())
# REVOLUTE JOINTS
for i in range(1, nbJoint + 1):
jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)]
jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName)
if i != nbJoint:
rmodel.appendBodyToJoint(jointId,firstLinkInertia, pinocchio.SE3.Identity())
else:
rmodel.appendBodyToJoint(jointId,secondLinkInertia, pinocchio.SE3.Identity())
rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT))
jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T)
rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0)
return rmodel
def createPendulum(self,
nbJoint,
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
jointPlacement = jointPlacement if jointPlacement != None else pin.SE3.Identity(
)
length = 1.0
mass = length
inertia = pin.Inertia(mass,
np.matrix([0.0, 0.0, length / 2]).T,
mass / 5 * np.diagflat([1e-2, length**2, 1e-2]))
for i in range(nbJoint):
istr = str(i)
name = prefix + "joint" + istr
jointName, bodyName = [name + "_joint", name + "_body"]
jointId = self.model.addJoint(jointId, pin.JointModelRY(),
jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia, pin.SE3.Identity())
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.]))))
model = pin.Model()
geom_model = pin.GeometryModel()
parent_id = 0
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 0.1
shape0 = fcl.Sphere(body_radius)
geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity())
geom0_obj.meshColor = np.array([1., 0.1, 0.1, 1.])
geom_model.addGeometryObject(geom0_obj)
for k in range(N):
joint_name = "joint_" + str(k + 1)
joint_id = model.addJoint(parent_id, pin.JointModelRY(), joint_placement,
joint_name)
body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
body_placement = joint_placement.copy()
body_placement.translation[2] = 1.
model.appendBodyToJoint(joint_id, body_inertia, body_placement)
geom1_name = "ball_" + str(k + 1)
shape1 = fcl.Sphere(body_radius)
geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1,
body_placement)
geom1_obj.meshColor = np.ones((4))
geom_model.addGeometryObject(geom1_obj)
geom2_name = "bar_" + str(k + 1)
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')
