Commit 99f36c37 authored by Galo Maldonado's avatar Galo Maldonado
Browse files

read .sto forces

parent 24cff894
......@@ -94,12 +94,31 @@ class Wrapper():
q.append( self.dof2pinocchio(data[i][:]) )
return q
def parseTrialVel(self, data):
for i in range(0, len(data)):
dq.append( self.vel2pinocchio(data[i][:]) )
return dq
def readOsim(self, filename):
extension = os.path.splitext(filename)[1]
if extension == '.mot':
print 'parsing motion data'
trial = osim_parser.readOsim(filename)
trial['osim_data'] = trial['data']
trial['pinocchio_data'] = np.asmatrix(self.parseTrial(trial['data'][:]))
trial['pinocchio_kine'] = np.asmatrix(self.parseTrialVel(trial['data'][:]))
trial['time'] = np.asmatrix(trial['time'][:])
return trial
elif extension == '.sto':
print 'parsing force data'
trial = np.asmatrix(osim_parser.readOsim(filename))
return trial
print 'could not parse, do not know the extension:'
print extension
def inverseDynamics(self, q, v, a, f_ext=None):
'''ID(q, v, a, f_ext)
......@@ -342,6 +361,7 @@ class Wrapper():
#Change OpenSim values to correpond to Pinocchio model
pt = np.squeeze(np.array( self.oMp * np.matrix(dof[3:6]).T )) #tx,ty,tz
#pt = np.squeeze(np.array( np.matrix(dof[3:6]).T )) #tx,ty,tz
pelvis = quaternion_from_matrix(euler_matrix((dof[2]),dof[0],dof[1],'szxy'))
#dof[39]=-dof[39]#wrist flexion l
#dof[40]=-dof[40]#wrist deviation l
......@@ -362,6 +382,12 @@ class Wrapper():
return q
def vel2pinocchio(self, dof):
#Change OpenSim values to correpond to Pinocchio model
pt = np.squeeze(np.array( self.oMp * np.matrix(dof[0:3]).T )) #tx,ty,tz
dq = np.array([ tuple(pt[0:3])+tuple(dof[0:3])+tuple(dof[6:42]) ])[0]
return dq
#test individual joints
def move(self, name, dof):
Supports Markdown
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