From cbaace3bbb187cc8a685e5040084c0c12671d950 Mon Sep 17 00:00:00 2001
From: Ludovic De Matteis <ldematteis@laas.fr>
Date: Tue, 24 Sep 2024 13:56:47 +0200
Subject: [PATCH] Add Baumgarte corrector for contact 6D

This corrects the desired contact acceleration, by accouting for drift
in position and velocity
---
 .../multibody/contacts/contact-6d-loop.hxx    | 25 +++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
index c80c08eb1..ba293d2ca 100644
--- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
+++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
@@ -109,6 +109,13 @@ void ContactModel6DLoopTpl<Scalar>::calc(
   d->f1af2 = d->f1Mf2.act(d->f2af2);
   d->a0 =
       (d->f1af1 - d->f1Mf2.act(d->f2af2) + d->f1vf1.cross(d->f1vf2)).toVector();
+
+  if (gains_[0] != 0.) {
+    d->a0 += gains_[0] * (-pinocchio::log6(d->f1Mf2).toVector());
+  }
+  if (gains_[1] != 0.) {
+    d->a0 += gains_[1] * (d->f1vf1 - d->f1vf2).toVector();
+  }
 }
 
 template <typename Scalar>
@@ -163,6 +170,24 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
           (joint2_placement_.toActionMatrixInverse() * d->a2_partial_dv) -
       d->f1vf2.toActionMatrix() * d->f1Jf1 +
       d->f1vf1.toActionMatrix() * d->f1Xf2 * d->f2Jf2;
+  if (gains_[0] != 0.) {
+    Matrix6s f1Mf2_log6;
+    pinocchio::Jlog6(d->f1Mf2, f1Mf2_log6);
+    d->da0_dx.leftCols(nv) +=
+        gains_[0] * (-f1Mf2_log6 * (-d->oMf2.toActionMatrixInverse() *
+                                        d->oMf1.toActionMatrix() * d->f1Jf1 +
+                                    d->f2Jf2));
+  }
+  if (gains_[1] != 0.) {
+    d->da0_dx.leftCols(nv) +=
+        gains_[1] *
+        (joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq -
+         d->f1Mf2.act(d->f2vf2).toActionMatrix() *
+             (d->f1Jf1 - d->f1Xf2 * d->f2Jf2) -
+         d->f1Xf2 * joint2_placement_.toActionMatrixInverse() *
+             d->v2_partial_dq);
+    d->da0_dx.rightCols(nv) += gains_[1] * (d->f1Jf1 - d->f1Xf2 * d->f2Jf2);
+  }
 }
 
 template <typename Scalar>