Commit 7fd292ea authored by SKURIC Antun's avatar SKURIC Antun
Browse files

using submodules

parent 96854c2f
[submodule "pyosim"]
path = pyosim
url = https://gitlab.inria.fr/human/pyosim
[submodule "opensim_models"]
path = opensim_models
url = https://gitlab.inria.fr/human/opensim_models
Subproject commit 2577ea76707559055fb8057f1e747d7b99c1c114
Subproject commit 8d23af9aa4973c9d31c15c3157d62b49439828b5
This diff is collapsed.
# PyOsim
A simple python3 wrapper of the OpenSim library.
## Dependencies
There are two dependencies: `numpy` and of course `OpenSim`. Numpy you can install easily using pip:
```
pip install numpy
# or for python3
pip3 install numpy
# or in conda
conda install numpy
```
The prefered python version is python3 but it will work with python2 as well. The constraint will be the opensim installation.
## How to use it
Clone the github repository to your project:
```
cd my_project
git clone git@gitlab.inria.fr:human/pyosim.git
```
Your project structure will be something like this:
```
my_project
- my_code.py
- ...
- pyosim
- OsimModel.py
- amgebra.py
- __init__.py
- arm26.osim
```
And then in the code you just import
```python
from pyosim.OsimMode import OsimModel
```
## Code Docs
Find the full docs on the [link](./docs/README.md)
## Examples
### Display a model
Minimal code for displaying the opensim model
```python
from pyosim.OsimModel import OsimModel
## model path
model_path = 'your path to the model model.osim'
## Construct and display the model
OsimModel(model_path,visualize=True).displayModel()
```
### Display model states
Minimal code for displaying the opensim model
```python
from pyosim.OsimModel import OsimModel
model_path = 'your path to the model model.osim'
model = OsimModel(model_path,visualize=True)
# set model to neutral or random state
model.setToRandom()
# model.setToNeutral()
# print different model variables
model.printBodiesPositions()
model.printMarkersPositions()
model.printJointsValues()
# get model joint limits
print(model.getJointLimits())
# get muscle force limits
print(model.getForceLimits())
# print model jacobian
print(model.getJacobian(n=11))
# display model
model.displayModel()
```
# in __init__.py
\ No newline at end of file
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 = np.linalg.norm(q) #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):
raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
#point = np.real(Q[:, i[-1]]).squeeze()
#point /= point[3]
# rotation angle depending on direction
cosa = (np.trace(R33) - 1.0) / 2.0
if abs(direction[2]) > 1e-8:
sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
elif abs(direction[1]) > 1e-8:
sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
else:
sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
angle = math.atan2(sina, cosa)
return angle, direction#, point
def euler_from_rot_mat(rot_mat):
"""
Euler angles from rotation matrix ( 3x3 )
Parameters
----------
rot_mat : TYPE
DESCRIPTION.
Returns
-------
None.
"""
I = np.eye(4,4)
I[:3,:3] = rot_mat
ax,ay,az = euler_from_matrix(I)
return(ax,ay,az)
def inv_rpytoQUAT(r,p,y):
"""
Takes euler angles and return a quaternion
"""
euler_mat = euler_matrix(r,p,y)
quat = quaternion_from_matrix(euler_mat)
return(quat)
def q_mult(q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return np.array([w[0], x[0], y[0], z[0] ])