The dimension of the Mass Matrix and the bias force

[ami-iit]

The dimension of the Mass Matrix and the bias force

[traversaro]

I have another question. I'm trying to simulate a robot with 6 degrees of freedom (DOF), but I don't understand why I'm getting a 'NaN' (not a number) value. Here's my code: https://github.com/tommasoandina1/Doosan_h2515

Can you open a separate issue for this problem, and provide in it the output of your script, and the exact version of the installed dependencies, in particular casadi if you are using the casadi backend? Thanks! As a preliminary comment, I suggest you to also print the input to the dynamics to verify that there are no NaN in the inputs.

[Giulero]

Hi, it seems you are using at first symbolic types cs.sx and then converting the output of the function (which is an sx) in a numeric type cs.dm, precisely here cs.DM(M(H, s)).

Try to remove cs.DM and just print(M(H, s)). What's the output?

[Giulero]

The mass matrix depends on the robot configuration, namely $q:=(H,s)$. You have the inertial parameters from the urdf that are numbers, but you have the robot configuration that is symbolic. The result is a combination of inertial parameters and robot configuration values.

Have a look to

• https://www.diag.uniroma1.it/deluca/rob2_en/03_LagrangianDynamics_1.pdf
• https://www.diag.uniroma1.it/deluca/rob2_en/04_LagrangianDynamics_2.pdf

To inspect the mass matrix structure. These are for a manipulator, but for a floating-base robot the intuition is the same.

[traversaro]

I guess you are simulating a 6-DOF robot? A think to take in account is that the adam by default returns the floating base mass matrix, i.e. the one described in Equation 3.57 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf .

[tommasoandina1]

Yes I am using a robot with a DOF of 6, so I can directly exclude rows and columns from 7 to 12?

[tommasoandina1]

I have another question. I'm trying to simulate a robot with 6 degrees of freedom (DOF), but I don't understand why I'm getting a 'NaN' (not a number) value. Here's my code:
https://github.com/tommasoandina1/Doosan_h2515

[tommasoandina1]

@traversaro

[traversaro]

Yes I am using a robot with a DOF of 6, so I can directly exclude rows and columns from 7 to 12?

adam by default gives you the following equation of motions composed by (again, from Eq 3.57 https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf) 12 (6+6) rows:

$$ M(q) \dot{\nu} + C(\nu, q)\nu + G(q) = \begin{bmatrix} 0_{6 \times 1} \\ \tau \end{bmatrix} + \sum_i J_i^T f_i $$

where the position $q$ is composed by ${}^A H_B \in SE(3)$ and $s \in \mathbb{R}^{6 \times 1}$ (again, for more details check the aforementioned document)

If you want to simulate a fixed base robot, you need to apply the constraint

$$ {}^A H_B(t) = I_{4 \times 4} $$

that is equivalent to:

$$ {}^A \dot{H}_B(t) = 0_{4\times4} $$

$$ {}^{B[A]} \mathrm{v}_{A,B} = 0_{6\times1} $$

$$ {}^{B[A]} \dot{\mathrm{v}}_{A,B} = 0_{6\times1} $$

that means that only actual state that can change of the system are internal joint positions, velocities and accelerations ($s \in \mathbb{R}^{6 \times 1}$, $\dot{s} \in \mathbb{R}^{6 \times 1}$ and $\ddot{s} \in \mathbb{R}^{6 \times 1}$), so you can substitute in the equations:

$$ q = (I_{4 \times 4}, s) $$

$$ \nu = \begin{bmatrix} 0_{6\times1} \\ \dot{s} \end{bmatrix} = P \dot{s} $$

$$ \dot{\nu} = \begin{bmatrix} 0_{6\times1} \\ \ddot{s} \end{bmatrix} = P \ddot{s} $$

where $P \in \mathbb{R}^{12 \times 6}$ is the matrix defined as:

$$ P = \begin{bmatrix} 0_{6 \times 6} \ I_{6 \times 6} \end{bmatrix} $$

Substituting this equations in equations of motions and assuming that the only external force on the system is the constraint force acting on the base ${}^A f_0^x$, the dynamics get simplified as:

$$ M(s) P \ddot{s} + C(P\dot{s}, s) P \dot{s} + G(s) = \begin{bmatrix} {}^A f_0^x \\ \tau \end{bmatrix} $$

as you are probably not interested in the part of the dynamics related to ${}^A f_0^x$ (i.e. the ground reaction force) you can multiply all the equation by $P^T$, to obtain the usual 6-dof fixed-based dynamics:

$$ P^T M(s) P \ddot{s} + P^T C(P\dot{s}, s) P \dot{s} + P^T G(s) = \tau $$

or:

$$ M_{fixed}(s) \ddot{s} + n_{fixed}(\dot{s}, s) = \tau $$

[traversaro]

cc @Giulero

[tommasoandina1]

Yes I am using a robot with a DOF of 6, so I can directly exclude rows and columns from 7 to 12?

adam by default gives you the following equation of motions composed by (again, from Eq 3.57 https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf) 12 (6+6) rows:

M(q)ν˙+C(ν,q)ν+G(q)=[06×1τ]+∑iJiTfi

where the position q is composed by AHB∈SE(3) and s∈R6×1 (again, for more details check the aforementioned document)

If you want to simulate a fixed base robot, you need to apply the constraint

AHB(t)=I4×4

that is equivalent to:

AH˙B(t)=04×4

B[A]vA,B=06×1

B[A]v˙A,B=06×1

that means that only actual state that can change of the system are internal joint positions, velocities and accelerations (s∈R6×1, s˙∈R6×1 and s¨∈R6×1), so you can substitute in the equations:

q=(I4×4,s)

ν=[06×1s˙]=Ps˙

ν˙=[06×1s¨]=Ps¨

where P∈R12×6 is the matrix defined as:

P=[06×6 I6×6]

Substituting this equations in equations of motions and assuming that the only external force on the system is the constraint force acting on the base Af0x, the dynamics get simplified as:

M(s)Ps¨+C(Ps˙,s)Ps˙+G(s)=[Af0xτ]

as you are probably not interested in the part of the dynamics related to Af0x (i.e. the ground reaction force) you can multiply all the equation by PT, to obtain the usual 6-dof fixed-based dynamics:

PTM(s)Ps¨+PTC(Ps˙,s)Ps˙+PTG(s)=τ

or:

Mfixed(s)s¨+nfixed(s˙,s)=τ

I want to calculate the dynamics of robot

ν˙, so I used this formula:
ν˙ = M^-1(h-tau)

Where
h represents the bias force and tau denotes the couple at the joints, obtained from the control law. Initially, I was perplexed by the size of the mass matrix, which is 12x12. However, I later realized that only a submatrix of size 6x6 is pertinent. Another issue arose when I discovered that the Mass Matrix was populated with NaN values

[tommasoandina1]

I'm sorry but the formula was ν˙ = M^-1(tau-h)

[Giulero]

Hi @tommasoandina1! As @traversaro said, adam, by default, returns the floating-base mass matrix.
To have the mass matrix of a fixed-base robot you cannot find a better answer than @traversaro's one #77 (comment).

In order to help you, could you point us to the exact piece of code you're running with the output of your application?

[tommasoandina1]

Hi, there seems to be an issue here: https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan.ipynb. When I attempted to print the mass matrix and bias force, I don't understand why I'm getting a NaN value

[traversaro]

Hi, there seems to be an issue here: https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan.ipynb. When I attempted to print the mass matrix and bias force, I don't understand why I'm getting a NaN value

Can you print H and s to check if they have been correctly initialized?

[tommasoandina1]

Now I've updated the code
https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan.ipynb

[tommasoandina1]

Hi, there seems to be an issue here: https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan.ipynb. When I attempted to print the mass matrix and bias force, I don't understand why I'm getting a NaN value

Can you print H and s to check if they have been correctly initialized?

I'm only receiving variable values, not numerical ones

[Giulero]

From https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan.ipynb the output is the expected one.

I'm only receiving variable values, not numerical ones

Since you are using cs.SX, that's correct. Also the mass matrix output is the expected one since it is a function of symbolic values.

If you want numerical values when calling M(H,s), you should use numerical H and s.

[tommasoandina1]

But I'm using a robot model through a URDF file, so shouldn't I get a numerical value for my mass matrix?

[tommasoandina1]

In theory, I understood, so to calculate the values of the mass matrix and the mean error, do I have to calculate the heat from the Lagrangian or from the energy formula?

[Giulero]

Hi @tommasoandina1 ! I didn't get the question. What do you mean for mean error and heat from the Lagrangian?

For getting the mass matrix for a manipulator you should implement what @traversaro suggested you in #77 (comment).

Something like

# P is the "selector matrix" in https://github.com/ami-iit/adam/issues/77#issuecomment-2100398149
P = cs.horzcat(cs.SX.zeros(6,6), cs.SX.eye(6))

H = cs.SX.eye(4)
s = cs.SX.sym(6)

M_fixed_base = P.T @ M(H,s) @ P

# if you want to wrap in a cs.Function
M_fixed_base_fun = cs.Function("m_fun", [s], [M_fixed_base])

# and call it
s_num = np.random.randn(6) # if you want numerical values, otherwise use s = cs.SX.sym 
m = M_Fixed_base_fun(s_num) 

and to similar for the Coriolis and gravity term.

I do not know if this answers your question.

Note that I wrote this piece of code without testing it. So there might be errors

[tommasoandina1]

I would like to compute the accelerations of the robot, starting from joint angles, velocities, and torques. To do this, I shouldn't
While with mean error I mean that I would like to calculate desired positions relative to the obtained position, so I should calculate a numerical value rather than a symbolic one and I believe this is not possible with CasADi, shouldn't I use numpy?

[Giulero]

Yup you can do it! Just pass numerical values to the functions ;)
Use cs.DM or numpy.

[tommasoandina1]

I managed to convert my values to numeric values but I don't understand why I don't convert to the desired location
https://github.com/tommasoandina1/Doosan_h2515/blob/main/doosan2.ipynb

[Giulero]

Hi @tommasoandina1, I didn't exactly get the problem. Can you elaborate more?

[tommasoandina1]

Hi @Giulero, I want to control my robot with a PD control law tau,
Where
tau = kp (q_des - q_n) + kd dq_n
q_n+1 = dtdq_n
dq_n+1 = dt*ddq_n

but I don’t understand why it doesn’t work. It does not go to the desired position when I calculate the real position using its dynamics
And ddq_n+1 =M^-1 *(tau-h)
Where h is the bias force and M is mass matrix

[Giulero]

Hi @tommasoandina1, sorry for the late reply. I did not get exactly. Can you elaborate more?