From ea346d32e903b6fbc36cf72450bc707099d6d175 Mon Sep 17 00:00:00 2001 From: Ludovic De Matteis Date: Tue, 8 Oct 2024 09:16:34 +0200 Subject: [PATCH 01/23] Add Contact 6D calc and calcDiff --- .../multibody/contacts/contact-6d-loop.cpp | 233 +++++++++++++++ .../python/crocoddyl/multibody/multibody.cpp | 1 + .../python/crocoddyl/multibody/multibody.hpp | 1 + .../multibody/contacts/contact-6d-loop.hpp | 280 ++++++++++++++++++ .../multibody/contacts/contact-6d-loop.hxx | 239 +++++++++++++++ .../multibody/contacts/multiple-contacts.hxx | 9 +- include/crocoddyl/multibody/fwd.hpp | 7 + 7 files changed, 762 insertions(+), 8 deletions(-) create mode 100644 bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp create mode 100644 include/crocoddyl/multibody/contacts/contact-6d-loop.hpp create mode 100644 include/crocoddyl/multibody/contacts/contact-6d-loop.hxx diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp new file mode 100644 index 0000000000..6259b8c5b3 --- /dev/null +++ b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp @@ -0,0 +1,233 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh +// Heriot-Watt University +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#include "crocoddyl/multibody/contacts/contact-6d-loop.hpp" + +#include "python/crocoddyl/multibody/multibody.hpp" +#include "python/crocoddyl/utils/copyable.hpp" + +namespace crocoddyl { +namespace python { + +void exposeContact6DLoop() { + bp::register_ptr_to_python >(); + + bp::class_ >( + "ContactModel6DLoop", + "Rigid 6D contact model.\n\n" + "It defines a rigid 6D contact models based on acceleration-based " + "holonomic constraints.\n" + "The calc and calcDiff functions compute the contact Jacobian and drift " + "(holonomic constraint) or\n" + "the derivatives of the holonomic constraint, respectively.", + bp::init, int, pinocchio::SE3, int, + pinocchio::SE3, pinocchio::ReferenceFrame, std::size_t, + bp::optional >( + bp::args("self", "state", "joint1_id", "joint1_placement", + "joint2_id", "joint2_placement", "ref", "nu", "gains"), + "Initialize the contact model.\n\n" + ":param state: state of the multibody system\n" + ":param joint1_id: reference joint id of the first contact frame\n" + ":param joint1_placement: Placement of the first contact frame\n" + ":param joint2_id: reference joint id of the second contact frames\n" + ":param joint2_placement: Placement of the second contact frame\n" + ":param ref: reference frames of contact\n" + ":param nu: dimension of control vector\n" + ":param gains: gains of the contact model (default " + "np.matrix([0.,0.]))")) + .def("calc", &ContactModel6DLoop::calc, bp::args("self", "data", "x"), + "Compute the 6D contact Jacobian and drift.\n\n" + "The rigid contact model throught acceleration-base holonomic " + "constraint\n" + "of the contact frame placement.\n" + ":param data: contact data\n" + ":param x: state point (dim. state.nx)") + .def("calcDiff", &ContactModel6DLoop::calcDiff, + bp::args("self", "data", "x"), + "Compute the derivatives of the 6D contact holonomic constraint.\n\n" + "The rigid contact model throught acceleration-base holonomic " + "constraint\n" + "of the contact frame placement.\n" + "It assumes that calc has been run first.\n" + ":param data: cost data\n" + ":param x: state point (dim. state.nx)") + .def("updateForce", &ContactModel6DLoop::updateForce, + bp::args("self", "data", "force"), + "Convert the Lagrangian into a stack of spatial forces.\n\n" + ":param data: cost data\n" + ":param force: force vector (dimension 6)") + .def("createData", &ContactModel6DLoop::createData, + bp::with_custodian_and_ward_postcall<0, 2>(), + bp::args("self", "data"), + "Create the 6D contact data.\n\n" + "Each contact model has its own data that needs to be allocated. " + "This function\n" + "returns the allocated data for a predefined cost.\n" + ":param data: Pinocchio data\n" + ":return contact data.") + .add_property( + "joint1_id", + bp::make_function(&ContactModel6DLoop::get_joint1_id, + bp::return_value_policy()), + "reference joint id of the first contact frame") + .add_property( + "joint1_placement", + bp::make_function(&ContactModel6DLoop::get_joint1_placement, + bp::return_value_policy()), + "Placement of the first contact frame") + .add_property( + "joint2_id", + bp::make_function(&ContactModel6DLoop::get_joint2_id, + bp::return_value_policy()), + "reference joint id of the second contact frame") + .add_property( + "joint2_placement", + bp::make_function(&ContactModel6DLoop::get_joint2_placement, + bp::return_value_policy()), + "Placement of the second contact frame") + .add_property( + "gains", + bp::make_function(&ContactModel6DLoop::get_gains, + bp::return_value_policy()), + "contact gains") + .def(CopyableVisitor()); + +#pragma GCC diagnostic pop + + bp::register_ptr_to_python >(); + + bp::class_ >( + "ContactData6DLoop", "Data for 6DLoop contact.\n\n", + bp::init( + bp::args("self", "model", "data"), + "Create 6DLoop contact data.\n\n" + ":param model: 6DLoop contact model\n" + ":param data: Pinocchio data")[bp::with_custodian_and_ward< + 1, 2, bp::with_custodian_and_ward<1, 3> >()]) + .add_property( + "v1_partial_dq", + bp::make_getter(&ContactData6DLoop::v1_partial_dq, + bp::return_internal_reference<>()), + "Jacobian of the spatial velocity of the first contact frame wrt q") + .add_property("a1_partial_dq", + bp::make_getter(&ContactData6DLoop::a1_partial_dq, + bp::return_internal_reference<>()), + "Jacobian of the spatial acceleration of the first contact " + "frame wrt q") + .add_property("a1_partial_dv", + bp::make_getter(&ContactData6DLoop::a1_partial_dv, + 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, + bp::return_internal_reference<>()), + "Jacobian of the spatial velocity of the second contact frame wrt q") + .add_property("a2_partial_dq", + bp::make_getter(&ContactData6DLoop::a2_partial_dq, + bp::return_internal_reference<>()), + "Jacobian of the spatial acceleration of the second " + "contact frame wrt q") + .add_property("a2_partial_dv", + bp::make_getter(&ContactData6DLoop::a2_partial_dv, + 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<>()), + "Derivative of the acceleration drift wrt x") + .add_property("da0_dq_t1", + bp::make_getter(&ContactData6DLoop::da0_dq_t1, + bp::return_internal_reference<>()), + "Derivative of the acceleration drift wrt q - part 1") + .add_property("da0_dq_t2", + bp::make_getter(&ContactData6DLoop::da0_dq_t2, + bp::return_internal_reference<>()), + "Derivative of the acceleration drift wrt q - part 2") + .add_property("da0_dq_t3", + bp::make_getter(&ContactData6DLoop::da0_dq_t3, + bp::return_internal_reference<>()), + "Derivative of the acceleration drift wrt q - part 3") + .add_property("j1Xf1", + bp::make_getter(&ContactData6DLoop::j1Xf1, + bp::return_internal_reference<>()), + "Placement of the first contact frame in the joint frame - " + "Action Matrix") + .add_property("j2Xf2", + bp::make_getter(&ContactData6DLoop::j2Xf2, + bp::return_internal_reference<>()), + "Placement of the second contact frame in the joint frame " + "- Action Matrix") + .add_property("f1Mf2", + bp::make_getter(&ContactData6DLoop::f1Mf2, + bp::return_internal_reference<>()), + "Relative placement of the contact frames") + .add_property("f1Xf2", + bp::make_getter(&ContactData6DLoop::f1Xf2, + bp::return_internal_reference<>()), + "Relative placement of the contact frames - Action Matrix") + .add_property("f1Jf1", + bp::make_getter(&ContactData6DLoop::f1Jf1, + bp::return_internal_reference<>()), + "Jacobian of the first contact frame") + .add_property("f2Jf2", + bp::make_getter(&ContactData6DLoop::f2Jf2, + bp::return_internal_reference<>()), + "Jacobian of the second frame in the joint frame") + .add_property("j1Jj1", + bp::make_getter(&ContactData6DLoop::j1Jj1, + bp::return_internal_reference<>()), + "Jacobian of the first joint in the joint frame") + .add_property("j2Jj2", + bp::make_getter(&ContactData6DLoop::j2Jj2, + bp::return_internal_reference<>()), + "Jacobian of the second contact frame") + .add_property("f1vf1", + bp::make_getter(&ContactData6DLoop::f1vf1, + bp::return_internal_reference<>()), + "Velocity of the first contact frame") + .add_property("f2vf2", + bp::make_getter(&ContactData6DLoop::f2vf2, + bp::return_internal_reference<>()), + "Velocity of the second contact frame") + .add_property( + "f1vf2", + bp::make_getter(&ContactData6DLoop::f1vf2, + bp::return_internal_reference<>()), + "Velocity of the second contact frame in the first contact frame") + .add_property("f1af1", + bp::make_getter(&ContactData6DLoop::f1af1, + bp::return_internal_reference<>()), + "Acceleration of the first contact frame") + .add_property("f2af2", + bp::make_getter(&ContactData6DLoop::f2af2, + bp::return_internal_reference<>()), + "Acceleration of the second contact frame") + .add_property( + "f1af2", + bp::make_getter(&ContactData6DLoop::f1af2, + bp::return_internal_reference<>()), + "Acceleration of the second contact frame in the first contact frame") + .def(CopyableVisitor()); +} + +} // namespace python +} // namespace crocoddyl diff --git a/bindings/python/crocoddyl/multibody/multibody.cpp b/bindings/python/crocoddyl/multibody/multibody.cpp index 57dcfca1c7..96cf782e06 100644 --- a/bindings/python/crocoddyl/multibody/multibody.cpp +++ b/bindings/python/crocoddyl/multibody/multibody.cpp @@ -56,6 +56,7 @@ void exposeMultibody() { exposeContact2D(); exposeContact3D(); exposeContact6D(); + exposeContact6DLoop(); exposeImpulse3D(); exposeImpulse6D(); } diff --git a/bindings/python/crocoddyl/multibody/multibody.hpp b/bindings/python/crocoddyl/multibody/multibody.hpp index 4eb73b0a1b..6e1f150092 100644 --- a/bindings/python/crocoddyl/multibody/multibody.hpp +++ b/bindings/python/crocoddyl/multibody/multibody.hpp @@ -61,6 +61,7 @@ void exposeContact1D(); void exposeContact2D(); void exposeContact3D(); void exposeContact6D(); +void exposeContact6DLoop(); void exposeImpulse3D(); void exposeImpulse6D(); void exposeMultibody(); diff --git a/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp new file mode 100644 index 0000000000..7dc1a54e9f --- /dev/null +++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp @@ -0,0 +1,280 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, +// Heriot-Watt University +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_LOOP_HPP_ +#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_LOOP_HPP_ + +#include +#include + +#include "crocoddyl/core/utils/deprecate.hpp" +#include "crocoddyl/multibody/contact-base.hpp" +#include "crocoddyl/multibody/fwd.hpp" + +namespace crocoddyl { + +template +class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef _Scalar Scalar; + typedef MathBaseTpl MathBase; + typedef ContactModelAbstractTpl Base; + typedef ContactData6DLoopTpl Data; + typedef StateMultibodyTpl StateMultibody; + typedef ContactDataAbstractTpl ContactDataAbstract; + typedef pinocchio::SE3Tpl SE3; + typedef pinocchio::ForceTpl Force; + typedef typename MathBase::Vector2s Vector2s; + typedef typename MathBase::Vector3s Vector3s; + typedef typename MathBase::VectorXs VectorXs; + typedef typename MathBase::Matrix3s Matrix3s; + typedef typename MathBase::Matrix6s Matrix6s; + + /** + * @brief Initialize the 6d contact model from joint and placements + * + * + * @param[in] state State of the multibody system + * @param[in] joint1_id Reference joint id of the first contact + * @param[in] joint1_placement Placement of the first contact with respect + * to the joint + * @param[in] joint2_id Reference joint id of the second contact + * @param[in] joint2_placement Placement of the second contact with + * respect to the joint + * @param[in] ref Reference frame of contact + * @param[in] nu Dimension of the control vector + * @param[in] gains Baumgarte stabilization gains + */ + ContactModel6DLoopTpl(boost::shared_ptr state, + const int joint1_id, const SE3 &joint1_placement, + const int joint2_id, const SE3 &joint2_placement, + const pinocchio::ReferenceFrame ref, + const std::size_t nu, + const Vector2s &gains = Vector2s()); + + /** + * @brief Initialize the 6d contact model from joint and placements + * + * + * @param[in] state State of the multibody system + * @param[in] joint1_id Reference joint id of the first contact + * @param[in] joint1_placement Placement of the first contact with respect + * to the joint + * @param[in] joint2_id Reference joint id of the second contact + * @param[in] joint2_placement Placement of the second contact with + * respect to the joint + * @param[in] ref Reference frame of contact + * @param[in] gains Baumgarte stabilization gains + */ + ContactModel6DLoopTpl(boost::shared_ptr state, + const int joint1_id, const SE3 &joint1_placement, + const int joint2_id, const SE3 &joint2_placement, + const pinocchio::ReferenceFrame ref, + const Vector2s &gains = Vector2s::Zero()); + + virtual ~ContactModel6DLoopTpl(); + + /** + * @brief Compute the 3d contact Jacobian and drift + * + * @param[in] data 3d contact data + * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ + * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ + */ + virtual void calc(const boost::shared_ptr &data, + const Eigen::Ref &x); + + /** + * @brief Compute the derivatives of the 6d contact holonomic constraint + * + * @param[in] data 6d contact data + * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ + * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ + */ + virtual void calcDiff(const boost::shared_ptr &data, + const Eigen::Ref &x); + + /** + * @brief Convert the force into a stack of spatial forces + * + * @param[in] data 6d contact data + * @param[in] force 6d force + */ + virtual void updateForce(const boost::shared_ptr &data, + const VectorXs &force); + + /** + * @brief Create the 6d contact data + */ + virtual boost::shared_ptr createData( + pinocchio::DataTpl *const data); + + /** + * @brief Return the reference the first contact frame parent joint + */ + const int get_joint1_id() const; + + /** + * @brief Return the reference the first contact frame placement + */ + const SE3 &get_joint1_placement() const; + + /** + * @brief Return the reference the second contact frame parent joint + */ + const int get_joint2_id() const; + + /** + * @brief Return the reference the second contact frame placement + */ + const SE3 &get_joint2_placement() const; + + /** + * @brief Return the Baumgarte stabilization gains + */ + const Vector2s &get_gains() const; + + /** + * @brief Print relevant information of the 6d contact model + * + * @param[out] os Output stream object + */ + virtual void print(std::ostream &os) const; + + protected: + using Base::id_; + using Base::nc_; + using Base::nu_; + using Base::state_; + using Base::type_; + + private: + int joint1_id_; //!< Reference joint id of the first contact + SE3 joint1_placement_; //!< Placement of the first contact with respect to + //!< the joint + int joint2_id_; //!< Reference joint id of the second contact + SE3 joint2_placement_; //!< Placement of the second contact with respect to + //!< the joint + Vector2s gains_; //!< Baumgarte stabilization gains +}; + +template +struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef _Scalar Scalar; + typedef MathBaseTpl MathBase; + typedef ContactDataAbstractTpl Base; + typedef typename MathBase::Matrix3s Matrix3s; + typedef typename MathBase::Matrix6xs Matrix6xs; + typedef typename MathBase::Matrix6s Matrix6s; + typedef typename MathBase::MatrixXs MatrixXs; + typedef typename pinocchio::SE3Tpl SE3; + typedef typename pinocchio::SE3Tpl::ActionMatrixType SE3ActionMatrix; + typedef typename pinocchio::MotionTpl Motion; + typedef typename pinocchio::ForceTpl Force; + + template