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')