Robotics Stack Exchange is a question and answer site for professional robotic engineers, hobbyists, researchers and students. It only takes a minute to sign up.
Sign up to join this community
I have a 4-DOF robot arm system with 4 revolute joints arranged in an open-chain fashion like below:
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.
What I am trying to do is calculate the center of mass Jacobian matrix of the arm. I found some related materials online Center of Mass Jacobian.But I am still not very sure about how to calculate it. Could anybody give me some hint? Thanks!
The method you mention from elysium-labs is perfectly functional. I try to clarify it a little bit and I give you some C++ like code.
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.
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!):
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.
Jacobian.block<3,1>(0,0) = z0.cross(pEE_L-p0);
Jacobian.block<3,1>(0,1) = z1.cross(pEE_L-p1);
Jacobian.block<3,1>(3,0) = z0;
Jacobian.block<3,1>(3,1) = z1;
.block<3,1>(0,1) means matrix block of size 3x1 starting from element 0,1.
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.
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:
Jacobian_CoM.block<3,1>(0,0) = (partial_coms(3,0)/total_mass) * (z0.cross (R0*(partial_coms.block<3,1>(0,0)-PB)-p0));
Jacobian_CoM.block<3,1>(0,1) = (partial_coms(3,1)/total_mass) * (z1.cross (R0*(partial_coms.block<3,1>(0,1)-PB)-p1));
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."
In my code, I was interested in linear velocity only and that's why CoM jacobian is $3\times n$.
A good source for understanding the concept of jacobian, its geometric interpretation, and the notation that I've used in this answer, is this set of slides.
