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.