Base component of Spatial Velocity Jacobian for a Free-Floating Base Robot is all Zeros

Question

I am trying to find the end-effector spatial velocity Jacobian for a robot with a free-floating base. Due to the free-floating base, the jacobian should contain a base component and a manipulator comment (see https://spart.readthedocs.io/en/latest/Tutorial_Kinematics.html#jacobians)

V_ee = end-effector spatial velocity
J_b = base jacobian component
J_m = manipulator jacobian component
v = generalized velocities

V_ee = [J_b, J_m] v

Until now, I was using SPART toolbox to do this in Matlab (https://github.com/NPS-SRL/SPART) and now I am moving to Drake. I tried using CalcJacobianSpatialVelocity in the MultiBodyPlant and the manipulator Jacobian is correct when compared to SPART. However, the base component of the Jacobian is all zeros. This is different from what I expected and from SPART as for a free-floating base, the base velocities contribute to the end-effector spatial velocities.

An example reproduction of this issue can be found here: https://colab.research.google.com/github/vyas-shubham/DrakeTests/blob/main/freeFloating/computeJacobian.ipynb

I think I'm either doing one of these wrong while using Drake:

Using the CalcJacobianSpatialVelocity wrong. This is unlikely as the manipulator jacobian is correct and the base frame is also correct (only 1 frame in URDF).
Making a wrong URDF for calculating Jacobians for Free-Floating base. Maybe I need to specify differently in URDF a floating-base for Drake to include this in the Jacobian computation?

score 3 · Accepted Answer · answered Jan 03 '22 at 23:01

Your code is taking the Jacobian of the chaser relative to the target; there is no floating base between them (so the jacobian wrt to the floating base should, indeed, be zero). I think, perhaps, that you want to make frame_A=world_frame?

postCaptureTargetJacobian = postContactPlant.CalcJacobianSpatialVelocity(context=postCaptureContext,
                                             with_respect_to=JacobianWrtVariable.kV,
                                             frame_B=target_frame,
                                             p_BP=np.zeros(3),
                                             frame_A=world_frame,
                                             frame_E=world_frame)

PS - I added this cell to your notebook to quickly inspect your kinematic tree

from IPython.display import display, SVG
import pydot
display(SVG(pydot.graph_from_dot_data(postContactPlant.GetTopologyGraphvizString())[0].create_svg()))

PPS - Thanks for curating the example in the notebook. That made it easy for me to take a look.

Thanks! That does make sense. I see the mistake in my code/jacobian frames now. Changing frame_A=world_frame gives the same Jacobian as SPART toolbox for the floating base. — Shubham Vyas, Jan 04 '22 at 19:03

Base component of Spatial Velocity Jacobian for a Free-Floating Base Robot is all Zeros

1 Answers1