From 47f1672e9c6d844732b248d28be136c952a471c3 Mon Sep 17 00:00:00 2001
From: Ludovic De Matteis <ldematteis@laas.fr>
Date: Sat, 12 Oct 2024 00:07:18 +0200
Subject: [PATCH] Fix malloc in updateForce

---
 .../crocoddyl/multibody/contacts/contact-6d-loop.hpp |  5 +++++
 .../crocoddyl/multibody/contacts/contact-6d-loop.hxx | 12 ++++++++----
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
index 403669f94..10ce3af45 100644
--- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
+++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
@@ -245,6 +245,7 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
         da0_dq_t3_tmp(6, model->get_state()->get_nv()),
         dpos_dq(6, model->get_state()->get_nv()),
         dvel_dq(6, model->get_state()->get_nv()),
+        dtau_dq_tmp(model->get_state()->get_nv(), model->get_state()->get_nv()),
         f1Jf1(6, model->get_state()->get_nv()),
         f2Jf2(6, model->get_state()->get_nv()),
         f1Jf2(6, model->get_state()->get_nv()),
@@ -270,11 +271,13 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
     da0_dq_t3_tmp.setZero();
     dpos_dq.setZero();
     dvel_dq.setZero();
+    dtau_dq_tmp.setZero();
     f1Jf1.setZero();
     f2Jf2.setZero();
     f1Jf2.setZero();
     j1Jj1.setZero();
     j2Jj2.setZero();
+    j2Jj1.setZero();
     //
     j1Xf1 = SE3ActionMatrix::Identity();
     j2Xf2 = SE3ActionMatrix::Identity();
@@ -324,6 +327,7 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
 
   Matrix6xs dpos_dq;
   Matrix6xs dvel_dq;
+  MatrixXs dtau_dq_tmp;
   // Placement related data
   SE3 oMf1;   // Placement of the first contact frame in the world frame
   SE3 oMf2;   // Placement of the second contact frame in the world frame
@@ -337,6 +341,7 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
   Matrix6xs f1Jf2;
   Matrix6xs j1Jj1;
   Matrix6xs j2Jj2;
+  Matrix6xs j2Jj1;
   // Velocity related data
   Motion f1vf1;
   Motion f2vf2;
diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
index 20e749c8d..4a0016cea 100644
--- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
+++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
@@ -57,6 +57,11 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
                  "for 6D loop contacts"
               << std::endl;
   }
+  if (joint1_id == 0 || joint2_id == 0) {
+    std::cerr << "Warning: At least one of the parents joints id is zero"
+              "you should use crocoddyl::ContactModel6D instead"
+              << std::endl;
+  }
 }
 
 template <typename Scalar>
@@ -193,10 +198,9 @@ void ContactModel6DLoopTpl<Scalar>::updateForce(
 
   SE3 j2Mj1 =
       joint2_placement_.act(d->f1Mf2.actInv(joint1_placement_.inverse()));
-
-  d->dtau_dq.noalias() =
-      d->j2Jj2.transpose() *
-      (-f_cross * (d->j2Jj2 - j2Mj1.toActionMatrix() * d->j1Jj1));
+  d->j2Jj1.noalias() = j2Mj1.toActionMatrix() * d->j1Jj1;
+  d->dtau_dq_tmp.noalias() = -f_cross * (d->j2Jj2 - d->j2Jj1);
+  d->dtau_dq.noalias() = d->j2Jj2.transpose() * d->dtau_dq_tmp;
 }
 
 template <typename Scalar>