diff --git a/CHANGELOG.md b/CHANGELOG.md index fc312449fb..a904c4f3b1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +* Added 6D loop contacts to account for contacts between parts of the robot in https://github.com/loco-3d/crocoddyl/pull/1309 * Fixed the inequality constraints' feasibility computation by incorporating bounds into the calculation in https://github.com/loco-3d/crocoddyl/pull/1307 * Improved the action factory used for unit testing in https://github.com/loco-3d/crocoddyl/pull/1300 * Ignore ruff issues in ipython notebook files in https://github.com/loco-3d/crocoddyl/pull/1297 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..13693e4e3a --- /dev/null +++ b/bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp @@ -0,0 +1,242 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2024, 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", "type", "nu", "gains"), + "Initialize the contact model.\n\n" + ":param state: state of the multibody system\n" + ":param joint1_id: Parent joint id of the first contact frame\n" + ":param joint1_placement: Placement of the first contact frame with " + "respect to the parent joint\n" + ":param joint2_id: Parent joint id of the second contact frames\n" + ":param joint2_placement: Placement of the second contact frame with " + "respect to the parent joint\n" + ":param type: reference frame of contact (must be pinocchio::LOCAL)\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 loop-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 loop-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("updateForceDiff", &ContactModel6DLoop::updateForceDiff, + bp::args("self", "data", "df_dx", "df_du"), + "Update the force derivatives.\n\n" + ":param data: cost data\n" + ":param df_dx: Jacobian of the force with respect to the state\n" + ":param df_du: Jacobian of the force with respect to the control") + .def("createData", &ContactModel6DLoop::createData, + bp::with_custodian_and_ward_postcall<0, 2>(), + bp::args("self", "data"), + "Create the 6D loop-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()), + "Parent 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 with respect to the parent " + "joint") + .add_property( + "joint2_id", + bp::make_function(&ContactModel6DLoop::get_joint2_id, + bp::return_value_policy()), + "Parent 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 with respect to the parent " + "joint") + .add_property( + "gains", + bp::make_function(&ContactModel6DLoop::get_gains, + bp::return_value_policy()), + "Baumegarte stabilisation gains (Kp, Kd)") + .def(CopyableVisitor()); + + bp::register_ptr_to_python >(); + + bp::class_ >( + "ContactData6DLoop", "Data for 6DLoop contact.\n\n", + bp::init( + bp::args("self", "model", "data"), + "Create 6D loop-contact data.\n\n" + ":param model: 6D loop-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( + "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("da0_dx", + bp::make_getter(&ContactData6DLoop::da0_dx, + bp::return_internal_reference<>()), + "Jacobian of the acceleration drift wrt x") + .add_property("da0_dq_t1", + bp::make_getter(&ContactData6DLoop::da0_dq_t1, + bp::return_internal_reference<>()), + "Jacobian of the acceleration drift wrt q - part 1") + .add_property("da0_dq_t2", + bp::make_getter(&ContactData6DLoop::da0_dq_t2, + bp::return_internal_reference<>()), + "Jacobian of the acceleration drift wrt q - part 2") + .add_property("da0_dq_t3", + bp::make_getter(&ContactData6DLoop::da0_dq_t3, + bp::return_internal_reference<>()), + "Jacobian of the acceleration drift wrt q - part 3") + .add_property("f1Xj1", + bp::make_getter(&ContactData6DLoop::f1Xj1, + bp::return_internal_reference<>()), + "Inverse of the placement of the first contact frame in " + "the joint frame - " + "Action Matrix") + .add_property("f2Xj2", + bp::make_getter(&ContactData6DLoop::f2Xj2, + bp::return_internal_reference<>()), + "Inverse of the 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") + .add_property("joint1_f", + bp::make_getter(&ContactData6DLoop::joint1_f, + bp::return_internal_reference<>()), + "Force at the first joint") + .add_property("joint2_f", + bp::make_getter(&ContactData6DLoop::joint2_f, + bp::return_internal_reference<>()), + "Force at the second joint") + .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/contact-base.hpp b/include/crocoddyl/multibody/contact-base.hpp index 1b705f2cca..846548b15a 100644 --- a/include/crocoddyl/multibody/contact-base.hpp +++ b/include/crocoddyl/multibody/contact-base.hpp @@ -94,8 +94,9 @@ class ContactModelAbstractTpl { * @param[in] data Contact data * @param[in] force Contact force */ - void updateForceDiff(const boost::shared_ptr& data, - const MatrixXs& df_dx, const MatrixXs& df_du) const; + virtual void updateForceDiff( + const boost::shared_ptr& data, const MatrixXs& df_dx, + const MatrixXs& df_du) const; /** * @brief Set the stack of spatial forces to zero 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..457b66049c --- /dev/null +++ b/include/crocoddyl/multibody/contacts/contact-6d-loop.hpp @@ -0,0 +1,365 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2024, 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; + typedef typename MathBase::MatrixXs MatrixXs; + + /** + * @brief Initialize the 6d loop-contact model from joint and placements + * + * + * @param[in] state State of the multibody system + * @param[in] joint1_id Parent joint id of the first contact + * @param[in] joint1_placement Placement of the first contact with + * respect to the parent joint + * @param[in] joint2_id Parent joint id of the second contact + * @param[in] joint2_placement Placement of the second contact with + * respect to the parent joint + * @param[in] type 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 type const std::size_t nu, + const Vector2s &gains = Vector2s::Zero()); + + /** + * @brief Initialize the 6d loop-contact model from joint and placements + * + * + * @param[in] state State of the multibody system + * @param[in] joint1_id Parent joint id of the first contact + * @param[in] joint1_placement Placement of the first contact with + * respect to the parent joint + * @param[in] joint2_id Parent joint id of the second contact + * @param[in] joint2_placement Placement of the second contact with + * respect to the parent joint + * @param[in] type 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 type const Vector2s + &gains = Vector2s::Zero()); + + virtual ~ContactModel6DLoopTpl(); + + /** + * @brief Compute the 6d loop-contact Jacobian and drift + * + * @param[in] data 6d loop-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 loop-contact holonomic constraint + * + * @param[in] data 6d loop-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 loop-contact data + * @param[in] force 6d force + */ + virtual void updateForce(const boost::shared_ptr &data, + const VectorXs &force); + + /** + * @brief Updates the force differential for the given contact data. + * + * This function updates the force differential matrices with respect to the + * state and control variables. + * + * @param[in] data Shared pointer to the contact data abstract. + * @param[in] df_dx Matrix representing the differential of the force with + * respect to the state variables. + * @param[in] df_du Matrix representing the differential of the force with + * respect to the control variables. + */ + virtual void updateForceDiff( + const boost::shared_ptr &data, const MatrixXs &df_dx, + const MatrixXs &df_du); + + /** + * @brief Create the 6d loop-contact data + */ + virtual boost::shared_ptr createData( + pinocchio::DataTpl *const data); + + /** + * @brief Return the first contact frame parent joint + */ + const int get_joint1_id() const; + + /** + * @brief Return the first contact frame placement with respect to the parent + * joint + */ + const SE3 &get_joint1_placement() const; + + /** + * @brief Return the second contact frame parent joint + */ + const int get_joint2_id() const; + + /** + * @brief Return the second contact frame placement with respect to the parent + * joint + */ + const SE3 &get_joint2_placement() const; + + /** + * @brief Return the Baumgarte stabilization gains + */ + const Vector2s &get_gains() const; + + /** + * @brief Set the first contact frame parent joint + */ + void set_joint1_id(const int joint1_id); + + /** + * @brief Set the first contact frame placement with respect to the parent + * joint + */ + void set_joint1_placement(const SE3 &joint1_placement); + + /** + * @brief Set the second contact frame parent joint + */ + void set_joint2_id(const int joint2_id); + + /** + * @brief Set the second contact frame placement with respect to the parent + * joint + */ + void set_joint2_placement(const SE3 &joint2_placement); + + /** + * @brief Set the Baumgarte stabilization gains + */ + void set_gains(const Vector2s &gains); + + /** + * @brief Print relevant information of the 6D loop-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_; //!< Parent joint id of the first contact + SE3 joint1_placement_; //!< Placement of the first contact with respect to + //!< the parent joint + int joint2_id_; //!< Parent joint id of the second contact + SE3 joint2_placement_; //!< Placement of the second contact with respect to + //!< the parent 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