diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp index a75dd6034..b63403b54 100644 --- a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp +++ b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp @@ -165,15 +165,15 @@ void exposeContact6DLoop() { bp::make_getter(&ContactData6DLoop::da0_dq_t3, bp::return_internal_reference<>()), "Jacobian of the acceleration drift wrt q - part 3") - .add_property("j1Xf1", - bp::make_getter(&ContactData6DLoop::j1Xf1, + .add_property("f1Xj1", + bp::make_getter(&ContactData6DLoop::f1Xj1, bp::return_internal_reference<>()), - "Placement of the first contact frame in the joint frame - " + "Inverse of the placement of the first contact frame in the joint frame - " "Action Matrix") - .add_property("j2Xf2", - bp::make_getter(&ContactData6DLoop::j2Xf2, + .add_property("f2Xj2", + bp::make_getter(&ContactData6DLoop::f2Xj2, bp::return_internal_reference<>()), - "Placement of the second contact frame in the joint frame " + "Inverse of the placement of the second contact frame in the joint frame " "- Action Matrix") .add_property("f1Mf2", bp::make_getter(&ContactData6DLoop::f1Mf2, diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp index 068cf4cb7..63f08fe9b 100644 --- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp +++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp @@ -257,8 +257,8 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> { f1Jf2(6, model->get_state()->get_nv()), j1Jj1(6, model->get_state()->get_nv()), j2Jj2(6, model->get_state()->get_nv()), - j1Xf1(SE3ActionMatrix::Identity()), - j2Xf2(SE3ActionMatrix::Identity()), + f1Xj1(model->get_joint1_placement().inverse()), + f2Xj2(model->get_joint2_placement().inverse()), f1Mf2(SE3::Identity()), f1Xf2(SE3ActionMatrix::Identity()), f1vf1(Motion::Zero()), @@ -333,8 +333,8 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> { SE3 oMf2; // Placement of the second contact frame in the world frame SE3 f1Mf2; // Relative placement of the contact frames in the first contact // frame - SE3ActionMatrix j1Xf1; - SE3ActionMatrix j2Xf2; + SE3ActionMatrix f1Xj1; + SE3ActionMatrix f2Xj2; SE3ActionMatrix f1Xf2; // Jacobian related data Matrix6xs f1Jf1; diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx index 01b69dc8a..827a4590a 100644 --- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx +++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx @@ -72,17 +72,12 @@ void ContactModel6DLoopTpl::calc( const boost::shared_ptr &data, const Eigen::Ref &) { Data *d = static_cast(data.get()); - pinocchio::updateFramePlacements(*state_->get_pinocchio().get(), - *d->pinocchio); - d->j1Xf1.noalias() = joint1_placement_.toActionMatrix(); - d->j2Xf2.noalias() = joint2_placement_.toActionMatrix(); - pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio, joint1_id_, pinocchio::LOCAL, d->j1Jj1); pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio, joint2_id_, pinocchio::LOCAL, d->j2Jj2); - d->f1Jf1.noalias() = d->j1Xf1.inverse() * d->j1Jj1; - d->f2Jf2.noalias() = d->j2Xf2.inverse() * d->j2Jj2; + d->f1Jf1.noalias() = d->f1Xj1 * d->j1Jj1; + d->f2Jf2.noalias() = d->f2Xj2 * d->j2Jj2; d->oMf1 = d->pinocchio->oMi[joint1_id_].act(joint1_placement_); d->oMf2 = d->pinocchio->oMi[joint2_id_].act(joint2_placement_);