From f9de7b3e890f4238214f0eea93b952130c90595e Mon Sep 17 00:00:00 2001 From: Ludovic De Matteis Date: Wed, 16 Oct 2024 10:32:28 +0200 Subject: [PATCH] Apply review - Bindings and operation order Remove bindings for old data and change operation order for dvel_dq --- .../crocoddyl/multibody/contacts/contact-6d-loop.cpp | 10 ---------- .../crocoddyl/multibody/contacts/contact-6d-loop.hxx | 3 +-- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp index 70c2ee34f..a75dd6034 100644 --- 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 index d7737bceb..01b69dc8a 100644 --- a/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx +++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hxx @@ -174,9 +174,8 @@ void ContactModel6DLoopTpl::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; }