Gravity compensation is only calculated for the Jacobian of the foot. The resulting torque is applied for all links.
What I interpreted wrong here is that I apply this torque additively to the links, when I should had calculated new Jt for each link and applied the torque to all its parent links.
Gravity compensation is only calculated for the Jacobian of the foot. The resulting torque is applied for all links. What I interpreted wrong here is that I apply this torque additively to the links, when I should had calculated new Jt for each link and applied the torque to all its parent links.