Commit 8c18396d authored by Galo Maldonado's avatar Galo Maldonado
Browse files

update gitignore

parent 51470434
"""
.. module:: opensim parser with pinocchio
:platform: ubuntu
:parse OpenSim into pinocchio models
.. moduleauthor:: Galo Maldonado <galo_xav@hotmail.com>
"""
import numpy as np
import pinocchio as se3
import algebra as alg
from IPython import embed
def _parse2PinocchioJoints(pymodel):
jts = 0
# joint types
joint_models = []
# whether axis should be inverted L/R
axis_transformations = []
for joints in pymodel['Joints']:
dof_in_joint = 6 - (joints[2]['coordinates']).count(None)
if dof_in_joint == 6:
axis_transformations.append(joints[2]['axis'][0])
joint_models.append([jts, pymodel['Joints'][jts][0]['name'][0], se3.JointModelFreeFlyer()])
elif dof_in_joint == 3:
axis_transformations.append(joints[2]['axis'][0])
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:
axis_transformations.append(joints[2]['axis'][0])
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']:
joint_models.append([jts,pymodel['Joints'][jts][0]['name'][0],se3.JointModelRY()])
#Y
elif joints[2]['axis'][dof] == ['0', '1', '0']:
#Z
joint_models.append([jts,pymodel['Joints'][jts][0]['name'][0],se3.JointModelRZ()])
elif joints[2]['axis'][dof] == ['0', '0', '1']:
#X
joint_models.append([jts,pymodel['Joints'][jts][0]['name'][0],se3.JointModelRX()])
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])] )
#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
def pinocchioCoordinates(model, dof, representation="quat"):
oMp = se3.utils.rotate('z', np.pi/2) * se3.utils.rotate('x', np.pi/2)
q = np.matrix(np.zeros((model.nq, 1)))
qo_idx = 0 #osim index
qp_idx = 0 #pinocchio index
# for quaternions
def orderQuat(quat):
return [quat[1], quat[2], quat[3], quat[0]]
for i in xrange(1,len(model.joints)):
if (model.joints[i].shortname() == "JointModelFreeFlyer"):
q[qp_idx+0:qp_idx+3,0] = ( oMp * dof[qo_idx+3:qo_idx+6,0] ).A #tx,ty,tz
q[qp_idx+3:qp_idx+7,0] = np.matrix(orderQuat(alg.quaternion_from_matrix(alg.euler_matrix((dof[qo_idx,0]), dof[qo_idx+1,0], dof[qo_idx+2,0], 'rxyz')))).T
qo_idx += 6
qp_idx += 7
elif (model.joints[i].shortname() == "JointModelSpherical"):
q[qp_idx:qp_idx+4,0] = np.matrix(orderQuat(alg.quaternion_from_matrix(alg.euler_matrix((dof[qo_idx,0]), dof[qo_idx+1,0], dof[qo_idx+2,0], 'rxyz')))).T
qo_idx += 3
qp_idx += 4
elif (model.joints[i].shortname() == "se3.JointModelSphericalZYX"):
q[qp_idx:qp_idx+4,0] = np.matrix(orderQuat(alg.quaternion_from_matrix(alg.euler_matrix((dof[qo_idx,0]), dof[qo_idx+1,0], dof[qo_idx+2,0], 'rzyx')))).T
qo_idx += 3
qp_idx += 4
else:
q[qp_idx,0] = dof[qo_idx,0]
qo_idx += 1
qp_idx += 1
return q
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment