Apply review - Bindings and operation order

Remove bindings for old data and change operation order for dvel_dq
loco-3d · Oct 16, 2024 · f9de7b3 · f9de7b3
1 parent 5b883ba
commit f9de7b3
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 12 deletions.
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp
@@ -134,11 +134,6 @@ void exposeContact6DLoop() {
                                     bp::return_internal_reference<>()),
                     "Jacobian of the spatial acceleration of the first contact "
                     "frame wrt v")
-      .add_property("a1_partial_da",
-                    bp::make_getter(&ContactData6DLoop::a1_partial_da,
-                                    bp::return_internal_reference<>()),
-                    "Jacobian of the spatial acceleration of the first contact "
-                    "frame wrt a")
       .add_property(
           "v2_partial_dq",
           bp::make_getter(&ContactData6DLoop::v2_partial_dq,
@@ -154,11 +149,6 @@ void exposeContact6DLoop() {
                                     bp::return_internal_reference<>()),
                     "Jacobian of the spatial acceleration of the second "
                     "contact frame wrt v")
-      .add_property("a2_partial_da",
-                    bp::make_getter(&ContactData6DLoop::a2_partial_da,
-                                    bp::return_internal_reference<>()),
-                    "Jacobian of the spatial acceleration of the second "
-                    "contact frame wrt a")
       .add_property("da0_dx",
                     bp::make_getter(&ContactData6DLoop::da0_dx,
                                     bp::return_internal_reference<>()),

diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
@@ -174,9 +174,8 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
     d->f2_v2_partial_dq.noalias() =
         joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
     d->f1_v2_partial_dq.noalias() = d->f1Xf2 * d->f2_v2_partial_dq;
-    d->dvel_dq.noalias() = d->f1_v1_partial_dq;
+    d->dvel_dq.noalias() = d->f1_v1_partial_dq - d->f1_v2_partial_dq;
     d->dvel_dq.noalias() -= d->f1vf2.toActionMatrix() * d->Jc;
-    d->dvel_dq.noalias() -= d->f1_v2_partial_dq;
     d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->dvel_dq;
     d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->Jc;
   }