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_joint_constructor(self):
j1 = pin.JointModelRX()
self.assertTrue(j1.nq == 1)
jc1 = pin.JointModelComposite(j1)
self.assertTrue(jc1.nq == 1)
self.assertTrue(len(jc1.joints) == 1)
self.assertTrue(jc1.njoints == len(jc1.joints))
j2 = pin.JointModelRX()
self.assertTrue(j2.nq == 1)
jc2 = pin.JointModelComposite(j1, pin.SE3.Random())
self.assertTrue(jc2.nq == 1)
self.assertTrue(len(jc2.joints) == 1)
self.assertTrue(jc2.njoints == len(jc2.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 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.]))))
display.viewer.gui.addSphere('world/sphere', 1.0,color)
# Example of use of the class Display to create a cylinder visual object.
radius = 1.0
height = 1.0
display.viewer.gui.addCylinder('world/cylinder', radius,height,color)
# Example of use of the class display to place the previously-create object at random SE3 placements.
display.place("world/box147",se3.SE3.Random(),False)
display.place("world/sphere",se3.SE3.Random(),False)
display.place("world/cylinder",se3.SE3.Random())"""
# Example of creation of a simple robot model with one single revolute joint rotating around axis X.
model = se3.Model.BuildEmptyModel()
jointName = "first_joint" # Name of joint.
jointPlacement = se3.SE3.Random() # SE3 placement of the joint wrt chain init.
parent = 0 # Index of the parent (0 is the universe).
jointModel = se3.JointModelRX() # Type of the joint to be created.
jointId = model.addJoint(parent, jointModel, jointPlacement, jointName)
print('Model dimensions: {:d}, {:d}, {:d}'.format(model.nq, model.nv,
model.nbodies))
bodyInertia = se3.Inertia.Random() # Body mass/center of mass/inertia
model.appendBodyToJoint(jointId,
bodyInertia,
body_placement=se3.SE3.Identity()) # The last
# argument is likely to always be Identity.
# Example of how to create a 'Data' object from a 'Model' object.
data = model.createData()
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
parent_id = joint_id
else:
base_radius = 0.2
shape_base = fcl.Sphere(base_radius)
geom_base = pin.GeometryObject("base", 0, shape_base, pin.SE3.Identity())
geom_base.meshColor = np.array([1., 0.1, 0.1, 1.])
geom_model.addGeometryObject(geom_base)
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 0.1
for k in range(N):
joint_name = "joint_" + str(k + 1)
joint_id = model.addJoint(parent_id, pin.JointModelRX(), 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,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')