Verified Commit 5a0e1015 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: use new actInv of SE3 on Constraint

parent fbb1cc37
......@@ -135,10 +135,9 @@ namespace pinocchio
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
const SE3 Minv = M.inverse();
typename SE3::ActionMatrixType dual_action_matrix(M.toDualActionMatrix());
typename SE3::ActionMatrixType action_matrix(Minv.toActionMatrix());
ReturnType intermediate_result(dual_action_matrix*I);
typename SE3::ActionMatrixType action_matrix(M.toActionMatrixInverse());
ReturnType intermediate_result = dual_action_matrix*I;
ReturnType res = intermediate_result*action_matrix;
return res;
}
......
......@@ -189,7 +189,7 @@ namespace pinocchio
data.iMf[parent] = data.liMi[i]*data.iMf[i];
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
jmodel.jointCols(J_) = data.iMf[i].inverse().act(jdata.S()); // TODO: overload actInv
jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
}
};
......
......@@ -46,7 +46,7 @@ namespace pinocchio
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].inverse().act(jdata.S()); // TODO: avoid computing inverse
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());
}
}
......@@ -112,7 +112,7 @@ namespace pinocchio
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].inverse().act(jdata.S()); // TODO: avoid computing inverse
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S());
typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v());
......
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