How to calculate the center of mass Jacobian matrix of a robot arm - Robotics Stack Exchange most recent 30 from robotics.stackexchange.com 2019-12-14T09:17:06Z https://robotics.stackexchange.com/feeds/question/6842 https://creativecommons.org/licenses/by-sa/4.0/rdf https://robotics.stackexchange.com/q/6842 1 How to calculate the center of mass Jacobian matrix of a robot arm Zengrui https://robotics.stackexchange.com/users/9157 2015-03-22T04:35:44Z 2015-04-24T10:28:03Z <p>I have a 4-DOF robot arm system with 4 revolute joints arranged in an open-chain fashion like below:</p> <p><img src="https://i.stack.imgur.com/wU0VL.png" alt="4-DOF robotic arm"> </p> <p>Assume that each link’s mass is a point mass located at p_i and each link’s center of mass is at p_i.</p> <p>What I am trying to do is calculate the center of mass Jacobian matrix of the arm. I found some related materials online <a href="http://www.elysium-labs.com/robotics-corner/learn-robotics/biped-basics/com-jacobian/" rel="nofollow noreferrer">Center of Mass Jacobian</a>.But I am still not very sure about how to calculate it. Could anybody give me some hint? Thanks!</p> https://robotics.stackexchange.com/questions/6842/-/6879#6879 1 Answer by Pouya for How to calculate the center of mass Jacobian matrix of a robot arm Pouya https://robotics.stackexchange.com/users/2523 2015-03-25T09:45:39Z 2015-03-25T09:45:39Z <p>The method you mention from <a href="http://www.elysium-labs.com/robotics-corner/learn-robotics/biped-basics/com-jacobian/" rel="nofollow noreferrer">elysium-labs</a> is perfectly functional. I try to clarify it a little bit and I give you some C++ like code. </p> <p>Let me start by recapping on jacobian matrix itself: Jacobian matrix relates the joint rates to the linear and angular velocity of the end-effector (EE). In other words, jacobian expresses the contribution of each joint velocity to EE velocity. There are basically two ways of calculating jacobian: Analytically and Geometric. Analytical jacobian is partial derivatives while geometric jacobian is based on geometric interpretation of motion.</p> <p>In theory, you can calculate CoM jacobian by doing partial derivative but this a very tedious task and impossible in practice. So geometric jacobian is the way to go. Lets see the formula for geometric jacobian (no CoM yet!):</p> <p><img src="https://i.stack.imgur.com/hAhbH.png" alt="enter image description here"></p> <p>The upper row shows the contribution of each joint to the end-effector velocity and bottom row for the angular velocity. $P_i$ and $z_i$ are position and orientation of the joint, extracted from Denavit Hartenberg parameters. '$\times$' is cross product operator.</p> <pre><code>Jacobian.block&lt;3,1&gt;(0,0) = z0.cross(pEE_L-p0); Jacobian.block&lt;3,1&gt;(0,1) = z1.cross(pEE_L-p1); Jacobian.block&lt;3,1&gt;(3,0) = z0; Jacobian.block&lt;3,1&gt;(3,1) = z1; </code></pre> <p><code>.block&lt;3,1&gt;(0,1)</code> means matrix block of size 3x1 starting from element 0,1.</p> <p>When calculating the CoM jacobian, we do not consider the end-effector. In other words, you must calculate the contribution of each joint movement to the velocity of the center of mass. </p> <p>First, you need to calculate the partial CoM, i.e., the CoM of each kinematic chain w.r.t the base. For example, partial CoM if you consider only $l_1$, then only $l_1, l_2$, then $l_1, l_2, l_3$ and so on. The rational is that $\theta_1$ has a bigger contribution to CoM velocity comparing to $\theta_4$. Now that we have all the necessary information, we calculate the geometric CoM jacobian, similarly to the kinematic jacobian:</p> <pre><code>Jacobian_CoM.block&lt;3,1&gt;(0,0) = (partial_coms(3,0)/total_mass) * (z0.cross (R0*(partial_coms.block&lt;3,1&gt;(0,0)-PB)-p0)); Jacobian_CoM.block&lt;3,1&gt;(0,1) = (partial_coms(3,1)/total_mass) * (z1.cross (R0*(partial_coms.block&lt;3,1&gt;(0,1)-PB)-p1)); </code></pre> <p>In the above code, "Note that we had to rotate the partial COM to get it into the base links coordinate frame", hence $R_0$. Furthermore, "the resultant linear velocity should be scaled by the mass of the partial COM because the COM is the average of the multi-mass system and high velocities on smaller masses play a lesser role on the total velocity of the COM."</p> <p>In my code, I was interested in linear velocity only and that's why CoM jacobian is $3\times n$. </p> <p>A good source for understanding the concept of jacobian, its geometric interpretation, and the notation that I've used in this answer, is <a href="http://www.dis.uniroma1.it/~deluca/rob1_en/11_DifferentialKinematics.pdf" rel="nofollow noreferrer">this</a> set of slides.</p>