Update forces for 6D loop contacts

loco-3d · Sep 24, 2024 · 0f702b5 · 0f702b5
1 parent 865c7fd
commit 0f702b5
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 10 deletions.
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp
@@ -226,6 +226,18 @@ void exposeContact6DLoop() {
           bp::make_getter(&ContactData6DLoop::f1af2,
                           bp::return_internal_reference<>()),
           "Acceleration of the second contact frame in the first contact frame")
+      .add_property("f_cross",
+                    bp::make_getter(&ContactData6DLoop::f_cross,
+                                    bp::return_internal_reference<>()),
+                    "Cross product matrix")
+      .add_property("joint1_f",
+                    bp::make_getter(&ContactData6DLoop::joint1_f,
+                                    bp::return_internal_reference<>()),
+                    "Force at the first joint")
+      .add_property("joint2_f",
+                    bp::make_getter(&ContactData6DLoop::joint2_f,
+                                    bp::return_internal_reference<>()),
+                    "Force at the second joint")
       .def(CopyableVisitor<ContactData6DLoop>());
 }
 

diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
@@ -247,6 +247,9 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
     f1af1 = Motion::Zero();
     f2af2 = Motion::Zero();
     f1af2 = Motion::Zero();
+    //
+    joint1_f = Force::Zero();
+    joint2_f = Force::Zero();
   }
 
   using Base::a0;
@@ -291,6 +294,12 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
   Motion f1af1;
   Motion f2af2;
   Motion f1af2;
+  // Force related data
+  Force joint1_f;
+  Force joint2_f;
+  Force f_local;
+
+  Matrix6s f_cross;
 };
 
 }  // namespace crocoddyl

diff --git a/include/crocoddyl/multibody/contacts/multiple-contacts.hxx b/include/crocoddyl/multibody/contacts/multiple-contacts.hxx
@@ -7,6 +7,7 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 
+#include "contact-6d-loop.hpp"
 namespace crocoddyl {
 
 template <typename Scalar>
@@ -250,14 +251,25 @@ void ContactModelMultipleTpl<Scalar>::updateForce(
         const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
             force.segment(nc, nc_i);
         m_i->contact->updateForce(d_i, force_i);
+        ContactModel6DLoopTpl<Scalar>* c =
+            dynamic_cast<ContactModel6DLoopTpl<Scalar>*>(m_i->contact.get());
+        if (c != nullptr) {
+          ContactData6DLoopTpl<Scalar>* dc =
+              static_cast<ContactData6DLoopTpl<Scalar>*>(d_i.get());
+          const pinocchio::JointIndex joint1 = c->get_joint1_id();
+          const pinocchio::JointIndex joint2 = c->get_joint2_id();
+          data->fext[joint1] = dc->joint1_f;
+          data->fext[joint2] = dc->joint2_f;
+        } else {
 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
-        const pinocchio::JointIndex joint =
-            state_->get_pinocchio()->frames[d_i->frame].parentJoint;
+          const pinocchio::JointIndex joint =
+              state_->get_pinocchio()->frames[d_i->frame].parentJoint;
 #else
-        const pinocchio::JointIndex joint =
-            state_->get_pinocchio()->frames[d_i->frame].parent;
+          const pinocchio::JointIndex joint =
+              state_->get_pinocchio()->frames[d_i->frame].parent;
 #endif
-        data->fext[joint] = d_i->fext;
+          data->fext[joint] = d_i->fext;
+        }
       } else {
         m_i->contact->setZeroForce(d_i);
       }
@@ -276,14 +288,25 @@ void ContactModelMultipleTpl<Scalar>::updateForce(
         const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
             force.segment(nc, nc_i);
         m_i->contact->updateForce(d_i, force_i);
+        ContactModel6DLoopTpl<Scalar>* c =
+            dynamic_cast<ContactModel6DLoopTpl<Scalar>*>(m_i->contact.get());
+        if (c != nullptr) {
+          ContactData6DLoopTpl<Scalar>* dc =
+              static_cast<ContactData6DLoopTpl<Scalar>*>(d_i.get());
+          const pinocchio::JointIndex joint1 = c->get_joint1_id();
+          const pinocchio::JointIndex joint2 = c->get_joint2_id();
+          data->fext[joint1] = dc->joint1_f;
+          data->fext[joint2] = dc->joint2_f;
+        } else {
 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
-        const pinocchio::JointIndex joint =
-            state_->get_pinocchio()->frames[d_i->frame].parentJoint;
+          const pinocchio::JointIndex joint =
+              state_->get_pinocchio()->frames[d_i->frame].parentJoint;
 #else
-        const pinocchio::JointIndex joint =
-            state_->get_pinocchio()->frames[d_i->frame].parent;
+          const pinocchio::JointIndex joint =
+              state_->get_pinocchio()->frames[d_i->frame].parent;
 #endif
-        data->fext[joint] = d_i->fext;
+          data->fext[joint] = d_i->fext;
+        }
         nc += nc_i;
       } else {
         m_i->contact->setZeroForce(d_i);