Regression in createJacobianEndEffWorld?

[jviereck]

When creating a signal via createJacobianEndEffWorld, the computed Jacobian is in the LOCAL and not LOCAL_WORLD_ALIGNED frame. This used to work before, so I guess the code regressed somewhere.

The underlying function computeGenericEndeffJacobian was modified here:

https://github.com/stack-of-tasks/sot-dynamic-pinocchio/commit/8fdf588c333fca2fbd1daf9a169baedbef89f900

but I cannot spot anything in there.

Does someone has an idea where this regression could come from?

---

Here some example outputs to show the regression

robot_dg.createJacobianEndEffWorld('jac_cnt_FL', 'FL_FOOT')
np.array(robot_dg.jac_cnt_FL.value)

array([[ 0.46,  0.25, -0.85, -0.13,  0.12, -0.03, -0.05, -0.05, -0.16,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.96,  0.28,  0.15, -0.06,  0.19,  0.13,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.89, -0.13,  0.45,  0.07, -0.19, -0.19,  0.03, -0.12,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.46,  0.25, -0.85,  0.46,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.96,  0.28,  0.  ,  1.  ,  1.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.89, -0.13,  0.45,  0.89,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ]])

And for comparison from Pinocchio:

endeff_id = robot_pin.model.getFrameId('FL_FOOT')

pin.forwardKinematics(robot_pin.model, robot_pin.data, q, v)
pin.updateFramePlacement(robot_pin.model, robot_pin.data, endeff_id)
pin.computeJointJacobians(robot_pin.model, robot_pin.data, q)

print(pin.getFrameJacobian(robot_pin.model, robot_pin.data, endeff_id, pin.ReferenceFrame.LOCAL))

[[ 0.46  0.25 -0.85 -0.13  0.12 -0.03 -0.05 -0.05 -0.16  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.96  0.28  0.15 -0.06  0.19  0.13 -0.   -0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.89 -0.13  0.45  0.07 -0.19 -0.19  0.03 -0.12  0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.46  0.25 -0.85  0.46  0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    0.96  0.28  0.    1.    1.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.89 -0.13  0.45  0.89  0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]]

print(pin.getFrameJacobian(robot_pin.model, robot_pin.data, endeff_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED))

[[ 1.    0.    0.    0.   -0.11 -0.18  0.   -0.13 -0.07  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    1.    0.    0.11  0.    0.2   0.11  0.   -0.04  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    1.    0.18 -0.2   0.    0.09 -0.01  0.14  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    1.    0.    0.    1.    0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    1.    0.    0.    0.96  0.96  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    0.    1.    0.    0.28  0.28  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]]

[jviereck]

Here is my hypothesis what is happening:

The change 8fdf588 removed the calls for updating the pinocchio::framesForwardKinematics and forwardKinematicsSINTERN. This way, the data.oMf entries are not computed anymore. However, when using the local world aligned frame, then the following is called:

M.rotation() = m_data->oMf[fid].rotation() * M.rotation();

where the first part is then just the identity because data.oMf is not initialized. Therefore, the computed Jacobian is the same as in the local / not world aligned frame.

[jmirabel]

Hi,

forwardKinematicsSINTERN is supposed to be called by jacobiansSINTERN. I usually have doubts that dependencies work correctly because the code of TimeDependent is unreadable and I find its behavior non-natural. However, in this case, you would get incoherent values if framesForwardKinematics has not been called (transform are not initialized by default).

It is possible that I introduced a bug with world jacobians as we are not using them. This API is a mess.

What looks suspicious to me is line

if (!isLocal)
  M.rotation() = m_data->oMf[fid].rotation() * M.rotation();

The correct version may be (not sure):

if (!isLocal) {
  pinocchio::SE3 T;
  T.rotation() = m_data->oMf[fid].rotation()
  T.translation().setZero();
  M = T * M;
}

However, that wouldn't explain the failure you encounter.

[jmirabel]

You can try to call the placement of a frame of your robot to make sure the forward kinematics is actually called.

[jviereck]

Hi @jmirabel ,

thanks for your feedback! The forwardKinematicsSINTERN gets called. As it turned out there was another call to updateFramePlacement necessary to compute the oMf data.

You were right about the version with using an extra SE3 transformation T and then doing M = T * M - now the jacobian computed from getFrameJacobian(..., LOCAL_WORLD_ALIGNED) and the jacobian computed here agree.

I've created a PR #77 to fix this problem.

[jmirabel]

Thanks for the feedback. Did you check with a frame for which frame.placement.translation is not zero ?

[jviereck]

Yes, I've tested for a non-zero frame.placement.translation.

[jviereck]

This one got fixed in #77.