Skip to content

GitLab

Commit 1e95a8ef authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

packaging

parent 7f6c3be4
Pipeline #4333 failed with stages
in 38 seconds
Hide whitespace changes
Inline Side-by-side
MANIFEST.in 0 → 100644
View file @ 1e95a8ef
include LICENSE
include README.md
recursive-include ospi *
exampleOLI.py 100644 → 100755
View file @ 1e95a8ef
import sys
sys.path.append('ospi')
#!/usr/bin/env python
import subprocess
import time
#from ospi
import viewer_utils as vw
import wrapper as wr
import motion_parser as mtp
import ospi.motion_parser as mtp
import ospi.viewer_utils as vw
import ospi.wrapper as wr
# The path to the model meshes
mesh_path='/home/mboukhed/devel/GAIT_SOT_OLI/ospi/models/whole_body/obj'
mesh_path = 'models/whole_body/obj'
# The path to the model and the filename
filename='/home/mboukhed/devel/GAIT_SOT_OLI/ospi/models/whole_body/wholebodyOLI.osim'
# Create a wrapper specific to the whole-body model
filename = 'models/whole_body/wholebodyOLI.osim'
# Create a wrapper specific to the whole-body model
# The wrapper parse the OpenSim model and builds pinocchio model and data
wb_model = wr.Wrapper(filename, mesh_path, name='whole-body_model10')
# call the gepetto viewer server
gvs = subprocess.Popen(["./gepetto-viewer.sh","&"])
print 'Loading the viewer ...'
gvs = subprocess.Popen('gepetto-gui')
print('Loading the viewer ...')
time.sleep(2)
# Init the viewer and add the model to it
viewer = vw.Viewer('viewer',wb_model)
viewer.setVisibility(wb_model.name+"/floor", "ON")
# Init the viewer and add the model to it
viewer = vw.Viewer('viewer', wb_model)
viewer.setVisibility(wb_model.name + "/floor", "ON")
viewer.display(wb_model.q0, wb_model.name)
#See axis
#viewer.JointFrames(wb_model.name)
# See axis
# viewer.JointFrames(wb_model.name)
# parse motion:
time, q, colheaders, qOsim = mtp.parseMotion(wb_model.model, wb_model.joint_transformations, '/home/mboukhed/devel/GAIT_SOT_OLI/ospi/OLI_F_3.mot','quat')
time, q, colheaders, qOsim = mtp.parseMotion(wb_model.model, wb_model.joint_transformations, 'OLI_F_3.mot', 'quat')
t = 0.0
t = 0.0
def playMotions(first=0, last=1, step=3, t=0):
for i in range(first, last, step):
viewer.setVisibility("OLI", "ON")
viewer.display(q[i].T, wb_model.name)
#time.sleep(4)
playMotions(0,396,1,0.0025)
# time.sleep(4)
playMotions(0, 396, 1, 0.0025)
time.sleep(4)
gvs.terminate()
gepetto-viewer.sh deleted 100755 → 0
View file @ 7f6c3be4
#!/bin/sh -x
/opt/openrobots/bin/gepetto-gui
ospi/#motion_parser.py# deleted 100644 → 0
View file @ 7f6c3be4
"""
.. 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# deleted 100644 → 0
View file @ 7f6c3be4
"""
.. 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
setup.py 100644 → 100755
View file @ 1e95a8ef
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#!/usr/bin/env python2
from setuptools import setup, find_packages
from distutils.core import setup
setup(
name='ospi',
version='1.0',
description='This library contains scripts for working with OpenSim files and pinocchio software.',
version='1.0.1',
packages=['ospi'],
keywords='biomechanics OpenSim pinocchio',
url='https://github.com/GaloMALDONADO/ospi/',
license='GNU General Public License v3.0',
url='https://github.com/gepetto/ospi/',
author='Galo MALDONADO',
author_email='galo_xav@hotmail.com',
packages=find_packages(exclude=['models','doc','build']),
author_email='galo.maldonado@laas.fr',
license='LGPL',
install_requires=['numpy'],
classifiers=[
'Development Status :: 3 - Alpha',
'Intended Audience :: Science/Research',
'Programming Language :: Python',
'Programming Language :: Python :: 2.7',
],
install_requires=['numpy'],
)
src/#motion_parser.py# deleted 100644 → 0
View file @ 7f6c3be4
"""
.. 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
src/#utils.py# deleted 100644 → 0
View file @ 7f6c3be4
"""
.. 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
src/algebra.py deleted 100644 → 0
View file @ 7f6c3be4
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]))