How can I calculate Center of mass for each link of Robot?

Question

I want to calculate 'r'

I know , and the Position of(X, Z) But I don't know how to apply it in this robot.

Answer 1

Since it is not sure that your robot is in a plane or not, I would consider it in 3D.

You have all the geometrical dimensions of the robot. And I assume you have also all angles in all joints.

You need to calculate the X Y and Z coordinates of all centers of masses on the image, relative to the same coordinate system. It is important that they are computed relative to the same coordinate system. In order to compute them in the same coordinate system you need to know all the joint angles.

For example:

The X coordinate of the center of mass ($x_{CoG}$) will equal, as the formula says:

$x_{CoG} = \frac{\sum{m_i \cdot x_i}}{M}$

the same is true for all other coordinates:

$y_{CoG} = \frac{\sum{m_i \cdot y_i}}{M}$; $z_{CoG} = \frac{\sum{m_i \cdot z_i}}{M}$

If all y coordinates are 0 (i.e. the robot is in a plane) obviously $y_{coG}$ will also be zero.

You can define a transformation matrix for each joint. The transformation matrices should be set up according to the D-H convention. This is not a must. The only variable in the transformation matrix will be the joint angle. You will need a vector which points to the CoG from the joint. This will be a constant vector (without variables), form the joint to the CoG (as it seems $[r,0,0]$). Please note that if you are not using the D-H convention the vector coordinates are not valid!

Let the reference coordinate system be $CS_a$ in the ankle. Assuming you have a 2 dof joint in the "ankle" of the robot

$$ \begin{pmatrix} x_m5 \\ y_m5 \\ z_m5 \\ 1 \end{pmatrix} = T_a \cdot T_{ankle_1}(q_{a1}) \cdot T_{ankle2}(q_{a2}) \cdot T_{ll} \cdot T_{knee}(q_k) \cdot \begin{pmatrix} r_5 \\ 0 \\ 0 \\ 1 \end{pmatrix} $$

Where $T_a$ is the transformation matrix of the reference coordiante system ($CS_a$). If indeed this it the reference coordinat system then it equals the $4x4$ identiy matrix $I_{4x4}$. $T_{ankle1}$ is the Dh transformation matrix of the first ankle rotation $T_{ankle2}$ is the transformation matrix f the second ankle joint. $q_{a1}$ and $q_{a2}$ are the joint angles. The translation from the anle joint to the knee joint is sperated to a constant transformation matrix, $T_{ll}$, corresponding to the lower leg. $$ T_{ll} = \begin{pmatrix} 1 & 0 & 0 & L_6 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix} $$

Thre reason behing this speparation is that this way you can caluclate the coordinates of the CoG in the lower leg as: $$ \begin{pmatrix} x_m6 \\ y_m6 \\ z_m6 \\ 1 \end{pmatrix} = T_a \cdot T_{ankle_1}(q_{a1}) \cdot T_{ankle2}(q_{a2}) \cdot \begin{pmatrix} r_6 \\ 0 \\ 0 \\ 1 \end{pmatrix} $$

You can calcualte similarly all coordinates of the centers of masses expressed in the same coordinate system (in this case the ankle coordinate system) and the mass associated with each center of mass. Then you start multiplying the masses with the coordinates and summing them up, divided by the total mass (similar to the same equation above). In your case 7 times.

However, I must add that this formula is used for calculating the center of mass of rigid bodies. I am not sure how well it performs for linkages.

When the robot is in motion, in order to calculate the positions of the CoGs, the same principles have to be applied, but cyclically.