Inertia matrix M(q) and Centripetal + Coriolis torques C(q, q•)

[HebiMigrator]

Hey Everyone!
We're working on a Daisy Hexapod and we're trying to do something with the torque data from the joints. For our application to be fruitful we need to find out the different terms of the standard equation for dynamic torque:

Tau = M(q)(q••) + C(q, q•)(q•) + g(q) + Tau_end-effector

Where M is the inertia matrix, C is the Coriolis and Centripetal matrix, g is the torque due to gravity, q is the angle of joints, q• is the angular velocity, q•• is the angular acceleration and Tau is the torque.

Which can be generated by the Euler-Lagrange equations formulated using the Lagrangian.

Is there any tool present to help in finding any of these out? The closest thing I could find was the getMasses function that outputs the location of the centre of masses of the links, in the body the body frame. Even using this, a lot of manual calculation is required to actually get the M and C matrices or form the Lagrangian, let alone differentiate it to get this equation.

Thanks for your help :),
Aditya Nair

Originally posted by AdityaNair on forums.hebi.us » Fri Aug 19, 2022 6:14 am

[HebiMigrator]

Hello Aditya, we are glad you are using one of the HEBI Robots.

We don't have a particular function that can calculate all parts of the equation of motions, but we do provide the tools to get you 90% of the way there.

To compute the effect of gravitational forces g(q) on the legs, you can use our gravity compensation torques to get the 3x1 external forces matrix. This should get a very good approximation.

Make sure you are on the newest version of the HEBI Python API.

''' Main Function:
get_grav_comp_efforts(positions: npt.NDArray[np.float64], gravity: npt.NDArray[np.floating[Any]], output=None)
'''

position = arm.last_feedback.position
gravity = [0,0,-9.81]
gravComp_Array = arm.robot_model.get_grav_comp_efforts(position, gravity, output=None)

We don't provide the manipulator inertia matrix M(q) of the individual modules and links. However, treating each as a point mass gets you most of the way there on the respective bodies. Then forming the M(q) matrix for the system is still a bit manual there but shouldn't be too hard.

The "get dynamics comp torques" logic we supply in the APIs and examples also takes care of most of this -- it basically provides to solution to M(q)(q**), and in that function, =the matrix we create is essentially M(q) (assuming point masses)

get_dynamic_comp_efforts(fbk_positions, cmd_positions, cmd_velocities, cmd_accels, dt=0.001)

You will have to manipulate or create a modify get_dynamic_comp_efforts function that gets the output to be just M(q) and not M(q)(q**) for your use case. The original get_dynamic_comp_efforts code is available here:

https://github.com/HebiRobotics/HEBI-Co ... l.py#L800

Lastly, with the relative masses and the speeds we are moving at, I believe the coriolis terms are inconsequential and can be assumed to be zero. There are much more significant sources of error here.

Link to HEBI Python ARM API: https://files.hebi.us/docs/python/2.6.1 ... mp_efforts

Let me know if you have anymore questions.

Originally posted by kamal_carter on forums.hebi.us » Fri Aug 19, 2022 4:32 pm

[HebiMigrator]

Thank you so much for your response. I apologize for not being able to reply earlier. It helped us a lot in our work and we greatly appreciate it, it makes a lot of stuff possible now!

Originally posted by AdityaNair on forums.hebi.us » Sat Oct 01, 2022 3:11 am