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 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 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(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 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 inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m**2)
def parseModel(filename, mesh_path, verbose=False):
pymodel = readOsim(filename)
ms_system = builder.MS("MS system") # TODO get model name
osMpi = se3.utils.rotate('z', np.pi/2) * se3.utils.rotate('x', np.pi/2)
joint_models, joint_transformations = utils._parse2PinocchioJoints(pymodel)
ms_system.createJointTransformations(joint_transformations)
idx = []
for joint in range(0,len(pymodel['Joints'])):
parent_name = pymodel['Joints'][joint][0]['parent_body'][0]
idx.append(parent_name)
jointLimits = []
for joint in range(0,len(pymodel['Joints'])):
# don't take into account ground body and visual
body = joint + 1
body_name = pymodel['Bodies'][body]['name'][0]
joint_name = pymodel['Joints'][joint][0]['name'][0]
joint_id = body
parent = idx.index(pymodel['Joints'][joint][0]['parent_body'][0])
joint_model = joint_models[joint][2]
jointLimits += pymodel['Joints'][joint][1]['range']
if (verbose):
print 'ID: ',joint_id
print 'Joint Name: '+joint_name
print 'Parent Name: '+pymodel['Joints'][joint][0]['parent_body'][0], parent
print 'Joint Model: ',joint_model
print 'Joint Limits: ',pymodel['Joints'][joint][1]['range']
# From OpenSim to Pinocchio
joint_placement = se3.SE3.Identity()
r = np.matrix(pymodel['Joints'][joint][0]['orientation_in_parent'][0],dtype = np.float64).T
# TODO change orientation of joint ***
joint_placement.rotation = se3.utils.rpyToMatrix(osMpi * r)
t = pymodel['Joints'][joint][0]['location_in_parent'][0]
joint_placement.translation = osMpi * np.matrix(t,dtype=np.float64).T
mass = np.float64(pymodel['Bodies'][body]['mass'][0])
mass_center = osMpi * np.matrix(pymodel['Bodies'][body]['mass_center'][0], dtype = np.float64).T
inertia_matrix = np.matrix(pymodel['Bodies'][body]['inertia'][0], dtype = np.float64)
body_inertia = (se3.Inertia( mass, mass_center, inertia_matrix))
body_placement = se3.SE3.Identity()
# Add to pinocchio model
ms_system.buildModel(
parent,
joint_model,
joint_placement,
joint_name,joint_id,
body_inertia,
body_placement,
body_name
)
scale_factors = osMpi * (np.matrix(pymodel['Visuals'][body][0]['scale_factors'][0], np.float64)).T
scale_factors = np.asarray(scale_factors.T)[0]
scale_factors = [scale_factors[0], scale_factors[1], scale_factors[2]]
# add to visuals list
for mesh in range(0, len(pymodel['Visuals'][body][1]['geometry_file']) ):
visual_name = os.path.splitext(pymodel['Visuals'][body][1]['geometry_file'][mesh])[0]
filename = mesh_path+'/'+visual_name+'.obj'
if (verbose): print 'Filename: '+filename
transform = np.matrix(pymodel['Visuals'][body][1]['transform'][mesh],dtype=np.float64).T
transform[3:6] = osMpi * transform[3:6]
transform[0:3] = osMpi * transform[0:3]
visuals = ms_system.createVisuals(parent, joint_name, filename, scale_factors, transform)
if (verbose): print '****'
# create data
ms_system.createData()
# add constraints
ms_system.createConstraints(np.matrix(jointLimits,dtype=np.float64))
# add forces
for force in range(0,len(pymodel['Forces'])):
force_name = pymodel['Forces'][force][0]['force_name'][0]
force_type = pymodel['Forces'][force][0]['type'][0]
points = []
for point in range(0,len(pymodel['Forces'][force][1])):
parent = pymodel['Forces'][force][1][point]['body'][0]
point_name = pymodel['Forces'][force][1][point]['point_name'][0]
location = osMpi * np.matrix([pymodel['Forces'][force][1][point]['location'][0]],dtype=np.float64).T
points.append([point_name,parent,location])
ms_system.createForces(force_name,force_type,parent,points)
return ms_system
def buildModel(self, PyModel=None, filename=None):
''' \ brief build a Pinocchio model given a PyModel
\param[in] PyModel The python model which can be created using readOsim. Default is None
\param[in] filename Path and name of the *.osim file which is used in case PyModel is not provided
\param[out] model Pinocchio model
'''
if PyModel is None:
# read osim model and store in python model
py = Osim2PythonModel()
PyModel = py.readModel(filename)
osMpi = se3.utils.rotate('z', np.pi / 2) * se3.utils.rotate(
'x', np.pi / 2)
joint_models = self.__OpenSimJointsToPynocchioJoints(PyModel)
id = []
for joint in range(0, len(PyModel['Joints'])):
parent_name = PyModel['Joints'][joint][0]['parent_body'][0]
id.append(parent_name)
for joint in range(0, len(PyModel['Joints'])):
# don't take into account ground body and visual
body = joint + 1
body_name = PyModel['Bodies'][body]['name'][0]
joint_name = PyModel['Joints'][joint][0]['name'][0]
joint_id = body
parent = id.index(PyModel['Joints'][joint][0]['parent_body'][0])
joint_model = joint_models[joint][2]
print 'ID: ', joint_id
print 'Joint Name: ' + joint_name
print 'Parent Name :' + PyModel['Joints'][joint][0]['parent_body'][
0], parent
print 'Joint Model: ', joint_model
print '****'
''' From OpenSim to Pinocchio
'''
joint_placement = se3.SE3.Identity()
r = np.matrix(
PyModel['Joints'][joint][0]['orientation_in_parent'][0],
dtype=np.float64).T
joint_placement.rotation = se3.utils.rpyToMatrix(osMpi * r)
t = PyModel['Joints'][joint][0]['location_in_parent'][0]
joint_placement.translation = osMpi * np.matrix(t,
dtype=np.float64).T
mass = np.float64(PyModel['Bodies'][body]['mass'][0])
mass_center = osMpi * np.matrix(
PyModel['Bodies'][body]['mass_center'][0], dtype=np.float64).T
inertia_matrix = np.matrix(PyModel['Bodies'][body]['inertia'][0],
dtype=np.float64)
body_inertia = (se3.Inertia(mass, mass_center, inertia_matrix))
body_placement = se3.SE3.Identity()
# Add to pynocchio model
self.model = self.builder.buildModel(parent, joint_model,
joint_placement, joint_name,
joint_id, body_inertia,
body_placement, body_name)
self.data = self.builder.createData()
scale_factors = osMpi * (np.matrix(
PyModel['Visuals'][body][0]['scale_factors'][0], np.float64)).T
scale_factors = np.asarray(scale_factors.T)[0]
scale_factors = [
scale_factors[0], scale_factors[1], scale_factors[2]
]
# add to visuals list
for mesh in range(
0, len(PyModel['Visuals'][body][1]['geometry_file'])):
visual_name = os.path.splitext(
PyModel['Visuals'][body][1]['geometry_file'][mesh])[0]
filename = config.mesh_path + '/' + visual_name + '.stl'
transform = np.matrix(
PyModel['Visuals'][body][1]['transform'][mesh],
dtype=np.float64).T
transform[3:6] = osMpi * transform[3:6]
transform[0:3] = osMpi * transform[0:3]
self.visuals = self.builder.createVisuals(
parent, joint_name, filename, scale_factors, transform)
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 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')
