Problem with Joints::COMbySegment function?

[yugiero]

Hi,

I want to compare the world frame trajectories of segments of biorbd to those of my own calculations. For this, I tried to use the function Joints::COMbySegments, which returns the CoM positions of all segments in the world frame as a list. However, when I try to access the different list elements, I always get back the CoM position of the base (pelvis/hips) segment. My model file is lukas.bioMod.txt

This is my code. As can be seen from the produced figures, hips_pos and foot_pos are exactly the same trajectories, even though different elements of the list were accessed in the loop before.

I hope the problem is clear, if not, pls lmk and I will try to come up with the minimum amount of code needed to demonstrate the problem.

Cheers, Lukas

[yugiero]

How to attach my model file correctly...? Github doesn't let me upload it. Each joint has 6 DoF in my definition. I also visualized the model in t-pose with bioviz, it has been correctly written

[pariterre]

Dear @yugiero

Would this be possible to copy-paste the code inside a triple ` bracket instead of providing a print screen? That would make it easier for me

I tested this code with a bioMod of mine

import biorbd
import numpy as np

m = biorbd.Model("MyModel.bioMod")
for i in range(m.nbSegment()):
    print(m.CoMbySegment(np.zeros(m.nbQ(), ))[i].to_array())

# Please note that the previous code is VERY slow because of the repeated calls to UpdateKinematics internally. This one does the same and is much faster: 
com_i = m.CoMbySegment(np.zeros(m.nbQ(), ))
for com in com_i:
    print(com.to_array())

This works just fine...

Are you sure that the CoMs for each segment are actually at different places?

[yugiero]

It works fine for me too. But how to load the q(t) trajectories into the model? I have to iterate over time steps, not over segments.

q = q_biorbd(joint_angles,base_trans)

fig,ax = plt.subplots(6,4,figsize=(12,18))
#plt.suptitle('Generalized Coordinates')
for i in range(len(segm_list)+1):
    ax[int(i/4),i%4].plot(q[i])
    #ax[int(i/4),i%4].set_title((['Hips Translation']+segm_list)[i])

plt.savefig('biorbd_joint_angles.jpg')

t = np.linspace(0,tsteps,tsteps-1)

plt.figure(dpi=400)
body_CoM,hips_pos,foot_pos = np.array([np.zeros((tsteps,3))]*3)
for i in range(tsteps):
    hips_pos[i] = lukas.CoMbySegment(q[:,i])[0].to_array()
    foot_pos[i] = lukas.CoMbySegment(q[:,i])[3].to_array()
plt.plot(hips_pos[:-1],label='biorbd hips')
plt.plot(foot_pos[:-1],label='biorbd rightfoot')
#plt.plot(D['Hips']['r'][:-1],label='own calcs')
#plt.plot(D['RightFoot']['r'][:-1])
plt.legend()

plt.savefig('segment position comparison.jpg')

[pariterre]

Hello again! I must admit, I do not fully grasp what the problem actually is... I think the main issue is that the figure you provided don't seem to be created from the script you sent. The reason I say that is because there only is 2 plt.plot for the second graph while the legend suggest 6 different things...

Or may it is supposed to be x,y,z? Do you mean the problem is that the values from HIP x,y,z are equal to the values of FOOT x,y,z? If so, please copy you bioMod as well and a striped version of your code (that is, loading the bioMod, loading the q (and print them so I can copy them too), then the for loop to get the CoM and plotting), without the extra stuff (like the savefig, or unrelated figures). So basically, as short as possible.

Like this I'll be able to better comprehend what is the actual problem.. Sorry for not exactly getting it :S

[yugiero]

No worries, you got it, each plt.plot is supposed to produce x,y,z graphs, and the weird thing is that the graphs from the "Hips" segment are exactly equal to the "RightFoot" ones, even though they should be different. Below are all files you need to execute the code. Feel free to ask any questions about the code, I know there are not enough comments..

#%%

import numpy as np
import matplotlib.pyplot as plt
import biorbd

#%%

tsteps = 289
segm_list = ['Hips','RightUpLeg','RightLeg',
              'RightFoot','RightToeBase','LeftUpLeg','LeftLeg',
             'LeftFoot','LeftToeBase','Spine','Spine1','Spine2',
              'Spine3','LeftShoulder','LeftArm','LeftForeArm',
             'LeftHand','RightShoulder','RightArm','RightForeArm',
             'RightHand','Neck','Head']

j_angle_file = 'filepath/jointangles.txt'
base_trans_file = 'filepath/basetrans.txt'

#%%

def q_biorbd(j_angle_file,base_trans_file):
    #joint angles:
    rot = np.loadtxt(j_angle_file).reshape((len(segm_list)+1,tsteps,3))
    #translation of floating base
    trans_hips = np.loadtxt(base_trans_file).reshape((1,tsteps,3))
    #all other transl DOFs are set to zero (rigid segm lengths)
    trans = np.concatenate((trans_hips,np.zeros((len(segm_list),tsteps,3))),axis=0)
    #interleaving rot and trans for each DOF
    q = np.zeros(((len(segm_list)+1)*2,tsteps,3))
    print(rot.shape)
    q[::2] = rot/360*2*np.pi  #from degrees to radians
    print(trans.shape)
    q[1::2] = trans
    #cutting off first and last element and reshaping:
    q = np.swapaxes(q[1:-1],axis1=2,axis2=1)
    print(q.shape)
    q = q.reshape((len(segm_list)*6,tsteps))
    print('Final shape for loading into biorbd: '+str(q.shape))
    return q

def q_plot(j_angle_file,base_trans_file):
    rot = np.loadtxt(j_angle_file).reshape((len(segm_list)+1,tsteps,3))
    trans = np.loadtxt(base_trans_file).reshape((1,tsteps,3))
    q = np.concatenate((trans,rot))
    q = np.delete(q,1,0)
    return q

#%%

m = biorbd.Model('lukas.bioMod')
q_for_model = q_biorbd(j_angle_file,base_trans_file)
hips_pos,foot_pos = np.array([np.zeros((tsteps,3))]*2)
for i in range(tsteps):
    hips_pos[i] = m.CoMbySegment(q_for_model[:,i])[0].to_array()
    foot_pos[i] = m.CoMbySegment(q_for_model[:,i])[3].to_array()

fig,ax = plt.subplots(6,4,figsize=(12,18))
for i in range(len(segm_list)+1):
    ax[int(i/4),i%4].plot(q_for_model[i])

#%%

plt.figure(dpi=400)
plt.plot(hips_pos[:-1],label='biorbd hips')
plt.plot(foot_pos[:-1],label='biorbd rightfoot')

#%%

q_for_plot = q_plot(j_angle_file,base_trans_file)

fig,ax = plt.subplots(6,4,figsize=(12,18))
for i in range(len(segm_list)+1):
    ax[int(i/4),i%4].plot(q_for_plot[i])
    ax[int(i/4),i%4].set_title((['Hips Translation']+segm_list)[i])

lukas.txt basetrans.txt jointangles.txt

[pariterre]

Hi! The issue is because of the root translation that you kept in mm while your model is defined in m. The q_biorbd function should read:

def q_biorbd(j_angle_file,base_trans_file):
    ...
    #translation of floating base
    trans_hips = np.loadtxt(base_trans_file).reshape((1,tsteps,3))
    trans_hips /= 1000
    ...

Then it produces larger differences for the positions (actually they are the same but with the right scale so you can see them)

Please note that you model kind of goes crazy (I animated it using bioviz with the following commands:

import bioviz
...
m = biorbd.Model('lukas.bioMod')
q_for_model = q_biorbd(j_angle_file,base_trans_file)
b = bioviz.Viz(loaded_model=m)
b.load_movement(q_for_model)
b.exec()

and it seems that the movement is a bit random... I would look into this if I were you!

Cheers

[yugiero]

Omg this is embarrassing :D it is actually in cm, idk why I didn't convert this... Ok I will have a look, thanks!

[yugiero]

I think the motions look random because I didn't add the floor constraint yet.