If I have a robot with generalized positions, velocities and accelerations as q, q_dot and q_ddot, I would like to compute the force that the end-effector applies on the environment in the current configuration.
In theory, the solution for this question would be the equation:
tau = (J_t)*F,
Where tau is the vector of generalized forces applied at the joints, J is the robot Jacobian (J_t the transpose), and F is the spatial force applied on the environment by the end-effector due to the motion of the robot.
How do I calculate the robot Jacobian matrix J using Drake?
Thank you for your time.
See the CalcJacobianSpatialVelocity methods of MultibodyPlant. You can find some prose around this in my course notes. We also provide CalcJacobianTranslationalVelocity, CalcJacobianAngularVelocity.
During a simulation, you may also wish to use MultibodyPlant's contact_results output port to extract the forces directly.