Skip to content

GitLab

Commit f3ecb732 authored by Galo Maldonado's avatar Galo Maldonado
Browse files

rename src to ospi to install with the package name

parent 397622d8
Expand all Hide whitespace changes
Inline Side-by-side
ospi/#motion_parser.py# 0 → 100644
View file @ f3ecb732
"""
.. module:: opensim parser compatible with numpy
:platform: ubuntu
:synopsis: parse OpenSim motion files into numpy matrices
.. moduleauthor:: Galo Maldonado <galo_xav@hotmail.com>
"""
import utils
import os
import xml.etree.ElementTree as xml
import numpy as np
import pinocchio as se3
from IPython import embed
def _readSto(filename, verbose=False):
"""This parser can be used with osim motion or force files
Note:
Does the same as parseMot
"""
return parseMot(filename, verbose)
def _readMot(filename, verbose=False):
"""This parser can be used with osim motion or force files
param filename: the complete filename. Accepted extensions are
".mot" and "sto".
type filename: str.
param verbose: false by default.
type: boolean.
returns: time numpy array, data numpy matrix and column headers
list of data
"""
data = []
time = []
file_extension = os.path.splitext(filename)[1][1:]
# Verify extension is correct
if file_extension not in ['mot', 'sto']:
print('File extension is not recognized. Only OpenSim .mot and .sto files can be parsed')
if (verbose): print 'File extension given is .'+file_extension
return
# Try to open the file
try:
f = open(filename, 'r')
except IOError:
print('cannot open', filename)
# Read the file
with open(filename, 'r') as f:
filename = f.readline().split()[0]
if (verbose): print('Reading file: '+filename)
# The header is not really informative
while True:
try:
line = f.readline().split()[0]
except IndexjError:
line = f.readline()
if line[0:9] == 'endheader':
break
# Get the colheaders
if (verbose): print("Reading the colheaders")
col_headers = f.readline().split()[1:]
# Read time and data from file
for rows in f:
data.append(rows.split()[1:])
time.append(float(rows.split()[0]))
for rows in range (0,len(data[:])):
for cols in range (0,len(data[0])):
if cols in (3,4,5):
# translations
data[rows][cols] = float(data[rows][cols])
else:
# store rotations in radians, defualt in opensim is degrees
data[rows][cols] = np.deg2rad(float(data[rows][cols]))
return np.array(time), np.matrix(data), col_headers
def parseMotion(model, filename, representation, verbose=False):
time, qOsim, col_headers = _readMot(filename, verbose)
q=[]
for t in xrange(0,time.size):
q.append(utils.pinocchioCoordinates(model, qOsim[t,:].T, representation))
return time, np.matrix(np.array(q).squeeze()), col_headers, qOsim
\ No newline at end of file
ospi/#utils.py# 0 → 100644
View file @ f3ecb732
"""
.. 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
ospi/algebra.py 0 → 100644
View file @ f3ecb732
import math
import numpy as np
#import scipy
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
def nullSpace(A, eps = 1e-15):
''' Computes the null space from SVD
returns the nulls space and the rank
'''
U, s, V = np.linalg.svd(A)
rank = (s>=eps).sum()
nullspace = V[rank:].T.copy()
return nullspace, rank
def svdDecomposition (A, threshold_value):
U, s, V = np.linalg.svd(A, full_matrices=False)
k = (s >= threshold_value).sum()
#print s
s_inv = 1./s[:k]
#print s_inv
#
U1 = np.matrix(U[:,:k])
V1 = V[:k,:].T
# Compute projector onto the Null Space
P = (-V[:k,:].T).dot(V[:k,:])
P.A[np.diag_indices_from(P)] += 1.
# Compute the pseudo inverse
A_pinv = V[:k,:].T.dot( np.diag(s_inv).dot(U[:,:k].T) )
#print k
#A_pinv = pinv(A,threshold_value)
#P = - A_pinv.dot(A)
#if type(P) is np.ndarray:
# P[np.diag_indices_from(P)] += 1.
#else:
# P.A[np.diag_indices_from(P)] += 1.
return A_pinv, P, k
def svdDecompositionBis (A, M, threshold_value):
#U, s, V = np.linalg.svd(A, full_matrices=False)
#k = (s >= threshold_value).sum()
#s_inv = 1./s[:k] #
#U1 = np.matrix(U[:,:k])
#V1 = V[:k,:].T
## Compute projector onto the Null Space
#P = (-V[:k,:].T).dot(V[:k,:])
#P.A[np.diag_indices_from(P)] += 1.
## Compute the pseudo inverse
#A_pinv = V[:k,:].T.dot( np.diag(s_inv).dot(U[:,:k].T) )
A_pinv = M.dot(A.T).dot(pinv(A.dot(M).dot(A.T),threshold_value))
P = - A_pinv.dot(A)
if type(P) is np.ndarray:
P[np.diag_indices_from(P)] += 1.
else:
P.A[np.diag_indices_from(P)] += 1.
return A_pinv, P
def dotproduct(v1, v2):
return sum((a * b) for a, b in zip(v1, v2))
def length(v):
return math.sqrt(dotproduct(v, v))
def angle(v1, v2):
return math.acos(dotproduct(v1, v2) / (length(v1) * length(v2)))
def euler_matrix(ai, aj, ak, axes='sxyz'):
"""Return homogeneous rotation matrix from Euler angles and axis sequence.
ai, aj, ak : Euler's roll, pitch and yaw angles
axes : One of 24 axis sequences as string or encoded tuple
>>> R = euler_matrix(1, 2, 3, 'syxz')
>>> np.allclose(np.sum(R[0]), -1.34786452)
True
>>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
>>> np.allclose(np.sum(R[0]), -0.383436184)
True
>>> ai, aj, ak = (4*math.pi) * (np.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
... R = euler_matrix(ai, aj, ak, axes)
>>> for axes in _TUPLE2AXES.keys():
... R = euler_matrix(ai, aj, ak, axes)
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
except (AttributeError, KeyError):
_TUPLE2AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]
if frame:
ai, ak = ak, ai
if parity:
ai, aj, ak = -ai, -aj, -ak
si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
cc, cs = ci*ck, ci*sk
sc, ss = si*ck, si*sk
M = np.identity(4)
if repetition:
M[i, i] = cj
M[i, j] = sj*si
M[i, k] = sj*ci
M[j, i] = sj*sk
M[j, j] = -cj*ss+cc
M[j, k] = -cj*cs-sc
M[k, i] = -sj*ck
M[k, j] = cj*sc+cs
M[k, k] = cj*cc-ss
else:
M[i, i] = cj*ck
M[i, j] = sj*sc-cs
M[i, k] = sj*cc+ss
M[j, i] = cj*sk
M[j, j] = sj*ss+cc
M[j, k] = sj*cs-sc
M[k, i] = -sj
M[k, j] = cj*si
M[k, k] = cj*ci
return M
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> np.allclose(M, np.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> np.allclose(M, np.diag([1, -1, -1, 1]))
True
"""
q = np.array(quaternion, dtype=np.float64, copy=True)
n = np.dot(q, q)
if n < _EPS:
return np.identity(4)
q *= math.sqrt(2.0 / n)
q = np.outer(q, q)
return np.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def quaternion_from_matrix(matrix, isprecise=False):
"""Return quaternion from rotation matrix.
If isprecise is True, the input matrix is assumed to be a precise rotation
matrix and a faster algorithm is used.
>>> q = quaternion_from_matrix(np.identity(4), True)
>>> np.allclose(q, [1, 0, 0, 0])
True
>>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))
>>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
True
>>> R = rotation_matrix(0.123, (1, 2, 3))
>>> q = quaternion_from_matrix(R, True)
>>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
True
>>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
>>> q = quaternion_from_matrix(R)
>>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
True
>>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
>>> q = quaternion_from_matrix(R)
>>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
True
>>> R = random_rotation_matrix()
>>> q = quaternion_from_matrix(R)
>>> is_same_transform(R, quaternion_matrix(q))
True
>>> R = euler_matrix(0.0, 0.0, np.pi/2.0)
>>> np.allclose(quaternion_from_matrix(R, isprecise=False),
... quaternion_from_matrix(R, isprecise=True))
True
"""
M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
if isprecise:
q = np.empty((4, ))
t = np.trace(M)
if t > M[3, 3]:
q[0] = t
q[3] = M[1, 0] - M[0, 1]
q[2] = M[0, 2] - M[2, 0]
q[1] = M[2, 1] - M[1, 2]
else:
i, j, k = 1, 2, 3
if M[1, 1] > M[0, 0]:
i, j, k = 2, 3, 1
if M[2, 2] > M[i, i]:
i, j, k = 3, 1, 2
t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
q[i] = t
q[j] = M[i, j] + M[j, i]
q[k] = M[k, i] + M[i, k]
q[3] = M[k, j] - M[j, k]
q *= 0.5 / math.sqrt(t * M[3, 3])
else:
m00 = M[0, 0]
m01 = M[0, 1]
m02 = M[0, 2]
m10 = M[1, 0]
m11 = M[1, 1]
m12 = M[1, 2]
m20 = M[2, 0]
m21 = M[2, 1]
m22 = M[2, 2]
# symmetric matrix K
K = np.array([[m00-m11-m22, 0.0, 0.0, 0.0],
[m01+m10, m11-m00-m22, 0.0, 0.0],
[m02+m20, m12+m21, m22-m00-m11, 0.0],
[m21-m12, m02-m20, m10-m01, m00+m11+m22]])
K /= 3.0
# quaternion is eigenvector of K that corresponds to largest eigenvalue
w, V = np.linalg.eigh(K)
q = V[[3, 0, 1, 2], np.argmax(w)]
if q[0] < 0.0:
np.negative(q, q)
return q
def euler_from_quaternion(quaternion, axes='sxyz'):
"""Return Euler angles from quaternion for specified axis sequence.
>>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
>>> np.allclose(angles, [0.123, 0, 0])
True
"""
return euler_from_matrix(quaternion_matrix(quaternion), axes)
def euler_from_matrix(matrix, axes='sxyz'):
"""Return Euler angles from rotation matrix for specified axis sequence.
axes : One of 24 axis sequences as string or encoded tuple
Note that many Euler angle triplets can describe one matrix.
>>> R0 = euler_matrix(1, 2, 3, 'syxz')
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
>>> R1 = euler_matrix(al, be, ga, 'syxz')
>>> np.allclose(R0, R1)
True
>>> angles = (4*math.pi) * (np.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
... R0 = euler_matrix(axes=axes, *angles)
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
... if not np.allclose(R0, R1): print(axes, "failed")
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
_TUPLE2AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
if repetition:
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
if sy > _EPS:
ax = math.atan2( M[i, j], M[i, k])
ay = math.atan2( sy, M[i, i])
az = math.atan2( M[j, i], -M[k, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2( sy, M[i, i])
az = 0.0
else:
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
if cy > _EPS:
ax = math.atan2( M[k, j], M[k, k])
ay = math.atan2(-M[k, i], cy)
az = math.atan2( M[j, i], M[i, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
def quaternion_about_axis(angle, axis):
"""Return quaternion for rotation about axis.
>>> q = quaternion_about_axis(0.123, [1, 0, 0])
>>> np.allclose(q, [0.99810947, 0.06146124, 0, 0])
True
"""
q = np.array([0.0, axis[0], axis[1], axis[2]])
qlen = vector_norm(q)
if qlen > _EPS:
q *= math.sin(angle/2.0) / qlen
q[0] = math.cos(angle/2.0)
return q
#Using the Euler-Rodrigues formula
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
theta = np.asarray(theta)
axis = axis/math.sqrt(np.dot(axis, axis))
a = math.cos(theta/2.0)
b, c, d = -axis*math.sin(theta/2.0)
aa, bb, cc, dd = a*a, b*b, c*c, d*d
bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
[2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])
def rpytoQUAT(r,p,y):
cr = np.cos(r)
cp = np.cos(p)
cy = np.cos(y)
sr = np.sin(r)
sp = np.sin(p)
sy = np.sin(y)
rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
[-sp, cp*sr, cp*cr]])
d0 = rotmat[0,0]
d1 = rotmat[1,1]
d2 = rotmat[2,2]
rr = 1.0+d0+d1+d2
if rr>0:
s = 0.5 / np.sqrt(rr)
_x = (rotmat[2,1] - rotmat[1,2]) * s
_y = (rotmat[0,2] - rotmat[2,0]) * s
_z = (rotmat[1,0] - rotmat[0,1]) * s
_r = 0.25 / s
else:
#Trace is less than zero, so need to determine which
#major diagonal is largest
if ((d0 > d1) and (d0 > d2)):
s = 0.5 / np.sqrt(1 + d0 - d1 - d2)
_x = 0.5 * s
_y = (rotmat[0,1] + rotmat[1,0]) * s
_z = (rotmat[0,2] + rotmat[2,0]) * s
_r = (rotmat[1,2] + rotmat[2,1]) * s
elif (d1 > d2):
s = 0.5 / np.sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,1] + rotmat[1,0]) * s
_y = 0.5 * s
_z = (rotmat[1,2] + rotmat[2,1]) * s
_r = (rotmat[0,2] + rotmat[2,0]) * s
else:
s = 0.5 / np.sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,2] + rotmat[2,0]) * s
_y = (rotmat[1,2] + rotmat[2,1]) * s
_z = 0.5 * s
_r = (rotmat[0,1] + rotmat[1,0]) * s
return (_x,_y,_z,_r)
def rotation_from_matrix(matrix):
"""Return rotation angle and axis from rotation matrix.
>>> angle = (random.random() - 0.5) * (2*math.pi)
>>> direc = numpy.random.random(3) - 0.5
>>> point = numpy.random.random(3) - 0.5
>>> R0 = rotation_matrix(angle, direc, point)
>>> angle, direc, point = rotation_from_matrix(R0)
>>> R1 = rotation_matrix(angle, direc, point)
>>> is_same_transform(R0, R1)
True
"""
R = np.array(matrix, dtype=np.float64, copy=False)
R33 = R[:3, :3]
# direction: unit eigenvector of R33 corresponding to eigenvalue of 1
w, W = np.linalg.eig(R33.T)
i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):
raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
direction = np.real(W[:, i[-1]]).squeeze()
# point: unit eigenvector of R33 corresponding to eigenvalue of 1
w, Q = np.linalg.eig(R)
i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):