Center of mass

Center of mass task map computes the center-of-mass of all of the robot links defined in the system model:

\[\Phi_\text{CoM}(\boldsymbol{x}) = \sum_i(\boldsymbol{P}_{\text{CoM}_i}^\text{world}m_i),\]

where \(\boldsymbol{P}_{\text{CoM}_i}^\text{world}\) is the position of the center-of-mass of the \(i\)-th link w.r.t. the world frame, and \(m_i\) is mass of the \(i\)-th body. The Jacobian is computed using the chain rule. This task map can also be initialized to compute the projection of the center-of-mass on the \(xy\)-plane. In this case, the \(z\)-component is removed.