# Finding Mass/Inertia Matrix of a robot

Hi I am trying to model my humanoid robot. The general EoM is

$$M(q)\ddot{q}+C(q,\dot{q})+G(q)=U$$

I know $$M(q)$$ is Inertia matrix which depends on joint positions $$\textbf{q}$$. However, I am having difficulty to find this matrix for my robot.

I searched on internet and found that in order to find the inertia matrix I have to use the relation between the kinetic energy of the robot and inertia matrix. Is it right? I don't know how these two quantities are related to each other.

• Welcome to Robotics Franky, but I'm afraid that it is not clear what you are asking. We prefer practical, answerable questions based on actual problems that you face, so it's a good idea to include details of what you want to achieve, what you tried, what you found & what you expected to find. Please take a look at How to Ask & tour for more information on how stack exchange works and work through the Robotics question checklist to edit your question to make it clearer. Mar 5 '19 at 11:13
• Hi @MarkBooth, thanks for letting me know that this is not clear. I will rewrite my question and try to make it clear. Mar 6 '19 at 4:21
• Thanks or the updated question Franky. I have re-opened it, and hopefully the new information will make it easier for people to answer. While people do often leave comments when they vote a question down, sometimes they don't have the time to help make a question better, or want to avoid a conflict if the vote is taken personally. Try not to think of a down vote as a personal attack, just think of a down vote as a suggestion that you consider how you can I make your question more clear, useful or well researched. Mar 6 '19 at 12:31
• Also, adding a 'thanks in advance' section to a question is not required, we all tend to be thankful for the people helping us, and expect other people to be thankful too, so saying it just adds noise to the question and distracts people from the problem posed. It may seem counter intuitive, but excessive politeness can itself be impolite, as giving people extra text to read, even if they ignore it, is disrespectful of their time. Mar 6 '19 at 12:42
• No problem, we all had to learn. Also... Not a forum. *8') Mar 6 '19 at 13:08

The inverse dynamics of a robot is given by the relationship

$$\tau = D^{-1}(q, \dot{q}, \ddot{q})$$

where $$\tau \in \mathbb{R}^N$$ is a vector of the required torque per joint and $$q \in \mathbb{R}^N$$ is a vector of joint coordinates. The function is a complex mixture of trig terms and robot kinematic and inertial parameters. Over the years various algorithms have been developed to compute it, most notably the recursive Newton-Euler algorithm (Luh, Walker and Paul 1983) which is $$O(N)$$.

Orin and Walker 1982 showed how the $$D^{-1}(\cdot)$$ algorithm can be used to compute $$M$$, $$C$$ and $$G$$ terms and this has complexity $$O(N^3)$$. Featherstone's articulated body method has complexity $$O(N)$$ but for $$N<9$$ has no advantage over the Orin and Walker method.

To make this tangible, here's an example using my Robotics Toolbox for MATLAB. We start by defining a model of a Puma560 robot (this model has full kinematic and inertial parameters)

>> mdl_puma560


which defines a SerialLink object called p560

>> p560
p560 =

Puma 560 [Unimation]:: 6 axis, RRRRRR, stdDH, fastRNE
- viscous friction; params of 8/95;
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|     1.5708|          0|
|  2|         q2|          0|     0.4318|          0|          0|
|  3|         q3|    0.15005|     0.0203|    -1.5708|          0|
|  4|         q4|     0.4318|          0|     1.5708|          0|
|  5|         q5|          0|          0|    -1.5708|          0|
|  6|         q6|          0|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+


we can examine the inertial parameters of link 1

>> p560.links(1).dyn
Revolute(std): theta=q, d=0, a=0, alpha=1.5708, offset=0
m    = 0
r    = 0           0           0
I    = | 0           0           0           |
| 0           0.35        0           |
| 0           0           0           |
Jm   = 0.0002
Bm   = 0.00148
Tc   = 0.395      (+) -0.435     (-)
G    = -62.61
qlim = -2.792527 to 2.792527


where m is the link mass, r is the centroid of the link wrt the link frame, I is the link inertia, Jm is the motor armature inertia, Bm is motor viscous friction, Tc is Coulomb friction and G is the gearbox ratio.

We'll pick a random joint angle and velocity vector

>> q = rand(1,6); qd = rand(1,6);


and now can compute the M, C and G terms

>> M = p560.inertia(q)
M =
2.7694   -0.6330   -0.0746    0.0011    0.0007    0.0000
-0.6330    4.3572    0.3227   -0.0006    0.0002    0.0000
-0.0746    0.3227    0.9380   -0.0007    0.0010    0.0000
0.0011   -0.0006   -0.0007    0.1925    0.0000    0.0000
0.0007    0.0002    0.0010    0.0000    0.1713    0.0000
0.0000    0.0000    0.0000    0.0000    0.0000    0.1941

>> C = p560.coriolis(q, qd)
C =
-0.2524   -0.1852    0.1824   -0.0014    0.0013   -0.0000
0.1388   -0.3675   -0.5775    0.0002   -0.0035    0.0000
-0.0055    0.2099   -0.0002   -0.0006   -0.0025    0.0000
-0.0005   -0.0002   -0.0000    0.0000    0.0004   -0.0000
0.0012    0.0016    0.0012   -0.0004   -0.0000   -0.0000
-0.0000    0.0000    0.0000    0.0000    0.0000         0

g =
-0.0000   14.7643   -7.4059    0.0114   -0.0205         0


Finding the Mass, Coriolis and Gravity Matrix for a robot, which is basically forward dynamics of the robot. Now, they are two very well-known techniques to solve this, mentioned in the Robotics Handbook as well as Robot modeling and Control book by Spong - 1) the Euler-Lagrange formulation that basically uses the kinetic and potential energy you're talking about, to derive the complete system dynamics and 2) the recursive Newton-Euler approach that recursively computes forces and corresponding torques on the end-effector and the joints respectively to obtain the dynamic equation. The final dynamic equation obtained in either of the approaches can be rearranged and compared with the standard equation you have mentioned in your question to obtain the MCG matrices.

However, both the approaches get difficult as the DOFs increase. Also, avoiding reinventing the wheel, I would suggest defining the URDF of the robot correctly with all individual link inertia tensors, COMs and the kinematic chain and then use the Orocos KDL library to directly obtain the MCG matrices. KDL already has implementations for general kinematics and dynamics principles and is pretty easy to use.

The KDL::ChainDynParam is the class class you should be looking to use.

The inertia matrix of a robot is important for creating a forward model. In a simulator like Gazebo the inertia matrix is given by an XML tag:

<inertial>
<mass value="5"/>
<inertia ixx="0.2" ixy="0.0" ixz="0.2" iyy="0.4" iyz="0.0" izz="0.4"/>
</inertial>


This allows the underlying physics engine to calculate the rigid body simulation correctly. If a framework outside of the ROS ecosystem is utilized, the parameters of the physics engine are determined by the user manual. It is even possible to calculate the parameters from the raw data. The is called system identification.

Predicting the position and the velocity of a dynamic system is more complicated than only put six numbers into an xml-tag. Especially if the robot contains of different parts like torso, wings and legs the overall system will move different from a simple ball which is flying around.

• I think in ROS Gazebo the inertia matrix you mentioned is for a single part (link) and not for whole body. Where as I believe $M(q)$ is inertia/mass matrix for whole body. Please correct me if I am wrong. Mar 7 '19 at 8:00