Review - change on jXf variables to be computed and stored more effic…

…iently
loco-3d · Nov 5, 2024 · 27a72e3 · 27a72e3
1 parent f9de7b3
commit 27a72e3
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 17 deletions.
diff --git 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
@@ -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
@@ -72,17 +72,12 @@ void ContactModel6DLoopTpl<Scalar>::calc(
     const boost::shared_ptr<ContactDataAbstract> &data,
     const Eigen::Ref<const VectorXs> &) {
   Data *d = static_cast<Data *>(data.get());
-  pinocchio::updateFramePlacements<Scalar>(*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_);