Commit 93138e71 authored by Julian Viereck's avatar Julian Viereck Committed by Olivier Stasse
Browse files

Fix regression in DynamicPinocchio::computeGenericEndeffJacobian

parent 74ccae42
Pipeline #10658 passed with stage
in 5 minutes and 20 seconds
...@@ -713,8 +713,16 @@ dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, c ...@@ -713,8 +713,16 @@ dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, c
jid = frame.parent; jid = frame.parent;
M = frame.placement.inverse(); M = frame.placement.inverse();
if (!isLocal) // Express the jacobian is world coordinate system.
M.rotation() = m_data->oMf[fid].rotation() * M.rotation(); if (!isLocal) {// Express the jacobian is world coordinate system.
// Need to compute frame placement for oMf.
pinocchio::updateFramePlacement(*m_model, *m_data, fid);
pinocchio::SE3 T;
T.rotation() = m_data->oMf[fid].rotation();
T.translation().setZero();
M = T * M;
}
} else { } else {
jid = (pinocchio::JointIndex)id; jid = (pinocchio::JointIndex)id;
if (!isLocal) { // Express the jacobian is world coordinate system. if (!isLocal) { // Express the jacobian is world coordinate system.
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment