@@ -264,15 +264,16 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
264264 dpos_dq(6 , model->get_state ()->get_nv()),
265265 dvel_dq(6 , model->get_state ()->get_nv()),
266266 dtau_dq_tmp(model->get_state ()->get_nv(), model->get_state()->get_nv()),
267+ f1Mf2(SE3::Identity()),
268+ f1Xj1(model->get_joint1_placement ().inverse()),
269+ f2Xj2(model->get_joint2_placement ().inverse()),
270+ f1Xf2(SE3ActionMatrix::Identity()),
267271 f1Jf1(6 , model->get_state ()->get_nv()),
268272 f2Jf2(6 , model->get_state ()->get_nv()),
269273 f1Jf2(6 , model->get_state ()->get_nv()),
270274 j1Jj1(6 , model->get_state ()->get_nv()),
271275 j2Jj2(6 , model->get_state ()->get_nv()),
272- f1Xj1(model->get_joint1_placement ().inverse()),
273- f2Xj2(model->get_joint2_placement ().inverse()),
274- f1Mf2(SE3::Identity()),
275- f1Xf2(SE3ActionMatrix::Identity()),
276+ j2Jj1(6 , model->get_state ()->get_nv()),
276277 f1vf1(Motion::Zero()),
277278 f2vf2(Motion::Zero()),
278279 f1vf2(Motion::Zero()),
0 commit comments