Commit 7f6c3be4 authored by Melya Boukheddimi's avatar Melya Boukheddimi
Browse files

Melya's updates

parent 1ddd1ec1
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -8,13 +8,15 @@ import viewer_utils as vw
import wrapper as wr
import motion_parser as mtp
# The path to the model meshes
mesh_path='/local/gmaldona/devel/Models/data/whole_body/obj'
mesh_path='/home/mboukhed/devel/GAIT_SOT_OLI/ospi/models/whole_body/obj'
# The path to the model and the filename
filename='/local/gmaldona/devel/Models/data/whole_body/wholebody.osim'
filename='/home/mboukhed/devel/GAIT_SOT_OLI/ospi/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_model1')
wb_model = wr.Wrapper(filename, mesh_path, name='whole-body_model10')
# call the gepetto viewer server
......@@ -24,16 +26,25 @@ time.sleep(2)
# Init the viewer and add the model to it
viewer = vw.Viewer('viewer',wb_model)
viewer.setVisibility(wb_model.name+"/floor", "OFF")
viewer.setVisibility(wb_model.name+"/floor", "ON")
viewer.display(wb_model.q0, wb_model.name)
# See kinematic ranges of motion
# See markers
# 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/gmaldona/Dropbox/kv03_filtered_ik.mot','quat')
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')
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)
#!/bin/sh -x
/local/gmaldona/git/hpp/gepetto-viewer-corba/build/bin/gepetto-viewer-server
/opt/openrobots/bin/gepetto-gui
File added
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
......@@ -79,7 +79,7 @@ def pinocchioCoordinates(model, joint_transformations, dof, representation="quat
jt_sph = jt[i-1]
tup = np.nonzero(jt_sph)
row=tup[0]; col=tup[1];
q[qp_idx:qp_idx+4,0] = np.matrix(orderQuat(alg.quaternion_from_matrix(alg.euler_matrix((dof[qo_idx,0])*jt_sph[row[0,0],col[0,0]], dof[qo_idx+1,0]*jt_sph[row[0,1],col[0,1]], dof[qo_idx+2,0]*jt_sph[row[0,2],col[0,2]], 'rxyz')))).T
q[qp_idx:qp_idx+4,0] = np.matrix(orderQuat(alg.quaternion_from_matrix(alg.euler_matrix((dof[qo_idx,0])*jt_sph[row[0],col[0]], dof[qo_idx+1,0]*jt_sph[row[1],col[1]], dof[qo_idx+2,0]*jt_sph[row[2],col[2]], 'rxyz')))).T
qo_idx += 3
qp_idx += 4
elif (model.joints[i].shortname() == "JointModelSphericalZYX"):
......
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