Commit 2313552c authored by jcarpent's avatar jcarpent
Browse files

[Algo] Improve Motion cross operation

parent 69d6a865
......@@ -183,7 +183,13 @@ namespace se3
// Spatial velocity of joint i expressed in the global frame o
const Motion ov(oMi.act(vJ));
jmodel.jointCols(data.dJ) = (ov.toActionMatrix() * jmodel.jointCols(data.J));
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock dJcols = jmodel.jointCols(data.dJ);
ColsBlock Jcols = jmodel.jointCols(data.J);
for(int k = 0; k < jmodel.nv(); ++k)
dJcols.col(k) = ov.cross(Motion(Jcols.col(k))).toVector();
Supports Markdown
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