Topic/contact 6 d closed loop redo #1345
+1,922
−595
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<boost::shared_ptr<ContactModel6DLoop> >(); | ||
|
|
||
| bp::class_<ContactModel6DLoop, bp::bases<ContactModelAbstract> >( | ||
| "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<boost::shared_ptr<StateMultibody>, int, pinocchio::SE3, int, | ||
| pinocchio::SE3, pinocchio::ReferenceFrame, std::size_t, | ||
| bp::optional<Eigen::Vector2d> >( | ||
| 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<bp::return_by_value>()), | ||
| "Parent joint id of the first contact frame") | ||
| .add_property( | ||
| "joint1_placement", | ||
| bp::make_function(&ContactModel6DLoop::get_joint1_placement, | ||
| bp::return_value_policy<bp::return_by_value>()), | ||
| "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<bp::return_by_value>()), | ||
| "Parent joint id of the second contact frame") | ||
| .add_property( | ||
| "joint2_placement", | ||
| bp::make_function(&ContactModel6DLoop::get_joint2_placement, | ||
| bp::return_value_policy<bp::return_by_value>()), | ||
| "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<bp::return_by_value>()), | ||
| "Baumegarte stabilisation gains (Kp, Kd)") | ||
| .def(CopyableVisitor<ContactModel6DLoop>()); | ||
|
|
||
| bp::register_ptr_to_python<boost::shared_ptr<ContactData6DLoop> >(); | ||
|
|
||
| bp::class_<ContactData6DLoop, bp::bases<ContactDataAbstract> >( | ||
| "ContactData6DLoop", "Data for 6DLoop contact.\n\n", | ||
| bp::init<ContactModel6DLoop*, pinocchio::Data*>( | ||
| 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<ContactData6DLoop>()); | ||
| } | ||
|
|
||
| } // namespace python | ||
| } // namespace crocoddyl |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,250 @@ | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
| // 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 <pinocchio/multibody/data.hpp> | ||
| #include <pinocchio/spatial/motion.hpp> | ||
|
|
||
| #include "crocoddyl/core/utils/deprecate.hpp" | ||
| #include "crocoddyl/multibody/contact-base.hpp" | ||
| #include "crocoddyl/multibody/fwd.hpp" | ||
|
|
||
| namespace crocoddyl { | ||
|
|
||
| template <typename _Scalar> | ||
| class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> { | ||
| public: | ||
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
|
|
||
| typedef _Scalar Scalar; | ||
| typedef ContactData6DLoopTpl<Scalar> Data; | ||
| typedef ContactModelAbstractTpl<Scalar> Base; | ||
| typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract; | ||
| typedef StateMultibodyTpl<Scalar> StateMultibody; | ||
| typedef pinocchio::SE3Tpl<Scalar> SE3; | ||
| typedef pinocchio::ForceTpl<Scalar> Force; | ||
| typedef MathBaseTpl<Scalar> MathBase; | ||
| 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<StateMultibody> 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<StateMultibody> 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<ContactDataAbstract> &data, | ||
| const Eigen::Ref<const VectorXs> &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<ContactDataAbstract> &data, | ||
| const Eigen::Ref<const VectorXs> &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<ContactDataAbstract> &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<ContactDataAbstract> &data, const MatrixXs &df_dx, | ||
| const MatrixXs &df_du); | ||
|
|
||
| /** | ||
| * @brief Create the 6d loop-contact data | ||
| */ | ||
| virtual boost::shared_ptr<ContactDataAbstract> createData( | ||
| pinocchio::DataTpl<Scalar> *const data); | ||
|
|
||
| const SE3& get_placement(const int force_index) const; | ||
|
|
||
| /** | ||
| * @brief Return the Baumgarte stabilization gains | ||
| */ | ||
| const Vector2s &get_gains() const; | ||
|
|
||
| /** | ||
| * @brief Set the first contact frame placement with respect to the parent | ||
| * joint | ||
| */ | ||
| void set_placement(const int force_index, const SE3& 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::nf_; | ||
| using Base::nu_; | ||
| using Base::placements_; | ||
| using Base::state_; | ||
| using Base::type_; | ||
|
|
||
| private: | ||
| Vector2s gains_; //!< Baumgarte stabilization gains | ||
| }; | ||
|
|
||
| template <typename _Scalar> | ||
| struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> { | ||
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
|
|
||
| typedef _Scalar Scalar; | ||
| typedef pinocchio::SE3Tpl<Scalar> SE3; | ||
| typedef typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType SE3ActionMatrix; | ||
| typedef pinocchio::MotionTpl<Scalar> Motion; | ||
| typedef pinocchio::ForceTpl<Scalar> Force; | ||
| typedef ContactDataAbstractTpl<Scalar> Base; | ||
| typedef MathBaseTpl<Scalar> MathBase; | ||
| typedef typename MathBase::Matrix3s Matrix3s; | ||
| typedef typename MathBase::Matrix6xs Matrix6xs; | ||
| typedef typename MathBase::Matrix6s Matrix6s; | ||
| typedef typename MathBase::MatrixXs MatrixXs; | ||
|
|
||
| template <template <typename Scalar> class Model> | ||
| ContactData6DLoopTpl(Model<Scalar> *const model, | ||
| pinocchio::DataTpl<Scalar> *const data) | ||
| : Base(model, data, 2), | ||
| da0_dq_t1(6, model->get_state()->get_nv()), | ||
| da0_dq_t2(6, model->get_state()->get_nv()), | ||
| da0_dq_t3(6, model->get_state()->get_nv()), | ||
| f1Mf2(SE3::Identity()), | ||
| f1Xf2(SE3ActionMatrix::Identity()), | ||
| f1vf2(Motion::Zero()), | ||
| f1af2(Motion::Zero()), | ||
| f_cross(6, 6) { | ||
| if (force_datas.size() != 2 || nf != 2) | ||
| throw_pretty( | ||
| "Invalid argument: " << "the force_datas has to be of size 2"); | ||
|
|
||
| ForceDataAbstract &fdata1 = force_datas[0]; | ||
| fdata1.frame = model->get_id(0); | ||
| fdata1.jMf = model->get_placement(0); | ||
| fdata1.fXj = fdata1.jMf.inverse().toActionMatrix(); | ||
| fdata1.type = model->get_type(); | ||
|
|
||
| ForceDataAbstract &fdata2 = force_datas[1]; | ||
| fdata2.frame = model->get_id(1); | ||
| fdata2.jMf = model->get_placement(1); | ||
| fdata2.fXj = fdata2.jMf.inverse().toActionMatrix(); | ||
| fdata2.type = model->get_type(); | ||
|
|
||
| da0_dq_t1.setZero(); | ||
| da0_dq_t2.setZero(); | ||
| da0_dq_t3.setZero(); | ||
| f_cross.setZero(); | ||
| } | ||
|
|
||
| using Base::a0; | ||
| using Base::da0_dx; | ||
| using Base::force_datas; | ||
| using Base::nf; | ||
|
|
||
| // Intermediate calculations | ||
| Matrix6xs da0_dq_t1; | ||
| Matrix6xs da0_dq_t2; | ||
| Matrix6xs da0_dq_t3; | ||
|
|
||
| // Coupled terms | ||
| SE3 f1Mf2; | ||
| SE3ActionMatrix f1Xf2; //<! Relative action matrix of the | ||
| // contact frames in the first contact frame | ||
| Motion f1vf2; | ||
| Motion f1af2; | ||
| Matrix6s f_cross; | ||
| }; | ||
|
|
||
| } // namespace crocoddyl | ||
| /* --- Details -------------------------------------------------------------- */ | ||
| /* --- Details -------------------------------------------------------------- */ | ||
| /* --- Details -------------------------------------------------------------- */ | ||
| #include "crocoddyl/multibody/contacts/contact-6d-loop.hxx" | ||
|
|
||
| #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_LOOP_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,276 @@ | ||
|
|
||
| /////////////////////////////////////////////////////////////////////////////// | ||
| // 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 <pinocchio/algorithm/frames.hpp> | ||
| #include <pinocchio/algorithm/kinematics-derivatives.hpp> | ||
|
|
||
| #include "crocoddyl/core/utils/exception.hpp" | ||
| #include "crocoddyl/multibody/contacts/contact-6d.hpp" | ||
|
|
||
| namespace crocoddyl { | ||
|
|
||
| template <typename Scalar> | ||
| ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl( | ||
| boost::shared_ptr<StateMultibody> 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) | ||
| : Base(state, pinocchio::ReferenceFrame::LOCAL, 2, 6, nu), gains_(gains) { | ||
| if (type != pinocchio::ReferenceFrame::LOCAL) | ||
| throw_pretty( | ||
| "Only reference frame pinocchio::LOCAL is supported " | ||
| "for 6D loop contacts"); | ||
| if (joint1_id == 0 || joint2_id == 0) | ||
| throw_pretty( | ||
| "Either joint1_id or joint2_id is set to 0, cannot use form a " | ||
| "kinematic loop with the world"); | ||
|
|
||
| id_[0] = joint1_id; | ||
| id_[1] = joint2_id; | ||
| placements_[0] = joint1_placement; | ||
| placements_[1] = joint2_placement; | ||
| type_ = type; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl( | ||
| boost::shared_ptr<StateMultibody> state, const int joint1_id, | ||
| const SE3 &joint1_placement, const int joint2_id, | ||
| const SE3 &joint2_placement, const pinocchio::ReferenceFrame type, | ||
| const Vector2s &gains) | ||
| : Base(state, pinocchio::ReferenceFrame::LOCAL, 2, 6), gains_(gains) { | ||
| if (type != pinocchio::ReferenceFrame::LOCAL) | ||
| throw_pretty( | ||
| "Only reference frame pinocchio::LOCAL is supported " | ||
| "for 6D loop contacts"); | ||
| if (joint1_id == 0 || joint2_id == 0) | ||
| throw_pretty( | ||
| "Either joint1_id or joint2_id is set to 0, cannot use form a " | ||
| "kinematic loop with the world"); | ||
|
|
||
| id_[0] = joint1_id; | ||
| id_[1] = joint2_id; | ||
| placements_[0] = joint1_placement; | ||
| placements_[1] = joint2_placement; | ||
| type_ = type; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| ContactModel6DLoopTpl<Scalar>::~ContactModel6DLoopTpl() {} | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::calc( | ||
| const boost::shared_ptr<ContactDataAbstract> &data, | ||
| const Eigen::Ref<const VectorXs> &) { | ||
| Data *d = static_cast<Data *>(data.get()); | ||
| // TODO(jfoster): is this frame placement update call needed? | ||
| pinocchio::updateFramePlacements(*state_->get_pinocchio().get(), | ||
| *d->pinocchio); | ||
|
|
||
| for (int i = 0; i < nf_; ++i) { | ||
| ForceDataAbstract &fdata_i = d->force_datas[i]; | ||
| pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio, | ||
| fdata_i.frame, pinocchio::LOCAL, fdata_i.jJj); | ||
| fdata_i.fJf = fdata_i.fXj * fdata_i.jJj; | ||
| fdata_i.oMf = d->pinocchio->oMi[id_[i]].act(fdata_i.jMf); | ||
| } | ||
|
|
||
| ForceDataAbstract &fdata_1 = d->force_datas[0]; | ||
| ForceDataAbstract &fdata_2 = d->force_datas[1]; | ||
|
|
||
| d->f1Mf2 = fdata_1.oMf.actInv(fdata_2.oMf); | ||
| d->f1Xf2 = d->f1Mf2.toActionMatrix(); | ||
| d->Jc = fdata_1.fJf - d->f1Xf2 * fdata_2.fJf; | ||
|
|
||
| // Compute the acceleration drift | ||
| for (int i = 0; i < nf_; ++i) { | ||
| ForceDataAbstract &fdata_i = d->force_datas[i]; | ||
| fdata_i.fvf = fdata_i.jMf.actInv(d->pinocchio->v[id_[i]]); | ||
| fdata_i.faf = fdata_i.jMf.actInv(d->pinocchio->a[id_[i]]); | ||
| } | ||
| d->f1vf2 = d->f1Mf2.act(fdata_2.fvf); | ||
| d->f1af2 = d->f1Mf2.act(fdata_2.faf); | ||
|
|
||
| d->a0 = | ||
| (fdata_1.faf - d->f1Mf2.act(fdata_2.faf) + fdata_1.fvf.cross(d->f1vf2)) | ||
| .toVector(); | ||
|
|
||
| if (gains_[0] != 0.0) | ||
| d->a0 += gains_[0] * -pinocchio::log6(d->f1Mf2).toVector(); | ||
| if (gains_[1] != 0.0) | ||
| d->a0 += gains_[1] * (fdata_1.fvf - d->f1vf2).toVector(); | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::calcDiff( | ||
| const boost::shared_ptr<ContactDataAbstract> &data, | ||
| const Eigen::Ref<const VectorXs> &) { | ||
| Data *d = static_cast<Data *>(data.get()); | ||
| const std::size_t nv = state_->get_nv(); | ||
| ForceDataAbstract &fdata_1 = d->force_datas[0]; | ||
| ForceDataAbstract &fdata_2 = d->force_datas[1]; | ||
|
|
||
| for (int i = 0; i < nf_; ++i) { | ||
| ForceDataAbstract &fdata_i = d->force_datas[i]; | ||
| fdata_i.faf = fdata_i.jMf.actInv(d->pinocchio->a[id_[i]]); | ||
| } | ||
| d->f1af2 = d->f1Mf2.act(fdata_2.faf); | ||
|
|
||
| for (int i = 0; i < nf_; ++i) { | ||
| ForceDataAbstract &fdata_i = d->force_datas[i]; | ||
|
|
||
| pinocchio::getJointAccelerationDerivatives( | ||
| *state_->get_pinocchio().get(), *d->pinocchio, fdata_i.frame, | ||
| pinocchio::LOCAL, fdata_i.v_partial_dq, fdata_i.a_partial_dq, | ||
| fdata_i.a_partial_dv, fdata_i.a_partial_da); | ||
| } | ||
|
|
||
| d->da0_dq_t1 = fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dq; | ||
|
|
||
| d->da0_dq_t2 = | ||
| d->f1af2.toActionMatrix() * (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) + | ||
| d->f1Xf2 * (fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dq); | ||
| d->da0_dq_t3 = | ||
| -d->f1vf2.toActionMatrix() * (fdata_1.jMf.toActionMatrixInverse() * | ||
| fdata_1.v_partial_dq) // part 1 | ||
| + fdata_1.fvf.toActionMatrix() * d->f1vf2.toActionMatrix() * | ||
| (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) // part 2 | ||
| + fdata_1.fvf.toActionMatrix() * d->f1Xf2 * | ||
| (fdata_2.jMf.toActionMatrixInverse() * | ||
| fdata_2.v_partial_dq); // part 3 | ||
|
|
||
| d->da0_dx.leftCols(nv) = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3; | ||
| d->da0_dx.rightCols(nv) = | ||
| fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dv - | ||
| d->f1Xf2 * (fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dv) - | ||
| d->f1vf2.toActionMatrix() * fdata_1.fJf + | ||
| fdata_1.fvf.toActionMatrix() * d->f1Xf2 * fdata_2.fJf; | ||
|
|
||
| if (gains_[0] != 0.0) { | ||
| Matrix6s f1Mf2_log6; | ||
| pinocchio::Jlog6(d->f1Mf2, f1Mf2_log6); | ||
| d->da0_dx.leftCols(nv) += | ||
| gains_[0] * | ||
| (-f1Mf2_log6 * (-fdata_2.oMf.toActionMatrixInverse() * | ||
| fdata_1.oMf.toActionMatrix() * fdata_1.fJf + | ||
| fdata_2.fJf)); | ||
| } | ||
| if (gains_[1] != 0.0) { | ||
| d->da0_dx.leftCols(nv) += | ||
| gains_[1] * | ||
| (fdata_1.jMf.toActionMatrixInverse() * fdata_1.v_partial_dq - | ||
| d->f1Mf2.act(fdata_2.fvf).toActionMatrix() * | ||
| (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) - | ||
| d->f1Xf2 * fdata_2.jMf.toActionMatrixInverse() * fdata_2.v_partial_dq); | ||
| d->da0_dx.rightCols(nv) += | ||
| gains_[1] * (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf); | ||
| } | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::updateForce( | ||
| const boost::shared_ptr<ContactDataAbstract> &data, const VectorXs &force) { | ||
| if (force.size() != 6) { | ||
| throw_pretty( | ||
| "Invalid argument: " << "lambda has wrong dimension (it should be 6)"); | ||
| } | ||
| Data *d = static_cast<Data *>(data.get()); | ||
| ForceDataAbstract &fdata_1 = d->force_datas[0]; | ||
| ForceDataAbstract &fdata_2 = d->force_datas[1]; | ||
|
|
||
| pinocchio::ForceTpl<Scalar> f = pinocchio::ForceTpl<Scalar>(-force); | ||
| switch (type_) { | ||
| case pinocchio::ReferenceFrame::LOCAL: { | ||
| // TODO(jfoster): unsure if this logic is correct | ||
| fdata_1.f = fdata_1.jMf.act(f); | ||
| fdata_1.fext = -fdata_1.jMf.act(f); | ||
| fdata_2.f = -(fdata_2.jMf * d->f1Mf2.inverse()).act(f); | ||
| fdata_2.fext = (fdata_2.jMf * d->f1Mf2.inverse()).act(f); | ||
|
|
||
| d->dtau_dq.setZero(); | ||
| d->f_cross.setZero(); | ||
| d->f_cross.topRightCorner(3, 3) = pinocchio::skew(fdata_2.fext.linear()); | ||
| d->f_cross.bottomLeftCorner(3, 3) = | ||
| pinocchio::skew(fdata_2.fext.linear()); | ||
| d->f_cross.bottomRightCorner(3, 3) = | ||
| pinocchio::skew(fdata_2.fext.angular()); | ||
|
|
||
| SE3 j2Mj1 = fdata_2.jMf.act(d->f1Mf2.actInv(fdata_1.jMf.inverse())); | ||
| d->dtau_dq = fdata_2.jJj.transpose() * (-d->f_cross * (fdata_2.jJj - j2Mj1.toActionMatrix() * fdata_1.jJj)); | ||
| break; | ||
| } | ||
| case pinocchio::ReferenceFrame::WORLD: | ||
| throw_pretty( | ||
| "Reference frame WORLD is not implemented for kinematic loops"); | ||
| case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: | ||
| throw_pretty( | ||
| "Reference frame LOCAL_WORLD_ALIGNED is not implemented for " | ||
| "kinematic loops"); | ||
| } | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::updateForceDiff( | ||
| const boost::shared_ptr<ContactDataAbstract> &data, const MatrixXs &df_dx, | ||
| const MatrixXs &df_du) { | ||
| if (static_cast<std::size_t>(df_dx.rows()) != nc_ || | ||
| static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx()) | ||
| throw_pretty("df_dx has wrong dimension"); | ||
|
|
||
| if (static_cast<std::size_t>(df_du.rows()) != nc_ || | ||
| static_cast<std::size_t>(df_du.cols()) != nu_) | ||
| throw_pretty("df_du has wrong dimension"); | ||
|
|
||
| data->df_dx = -df_dx; | ||
| data->df_du = -df_du; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| boost::shared_ptr<ContactDataAbstractTpl<Scalar>> | ||
| ContactModel6DLoopTpl<Scalar>::createData( | ||
| pinocchio::DataTpl<Scalar> *const data) { | ||
| return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, | ||
| data); | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::print(std::ostream &os) const { | ||
| os << "ContactModel6D {frame 1 = " | ||
| << state_->get_pinocchio()->frames[id_[0]].name | ||
| << ", frame 2 = " << state_->get_pinocchio()->frames[id_[1]].name | ||
| << ", type = " << type_ << ", gains = " << gains_.transpose() << "}"; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| const typename pinocchio::SE3Tpl<Scalar> & | ||
| ContactModel6DLoopTpl<Scalar>::get_placement(const int force_index) const { | ||
| return placements_[force_index]; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| const typename MathBaseTpl<Scalar>::Vector2s & | ||
| ContactModel6DLoopTpl<Scalar>::get_gains() const { | ||
| return gains_; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::set_gains( | ||
| const typename MathBaseTpl<Scalar>::Vector2s &gains) { | ||
| gains_ = gains; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| void ContactModel6DLoopTpl<Scalar>::set_placement( | ||
| const int force_index, | ||
| const typename pinocchio::SE3Tpl<Scalar> &placement) { | ||
| placements_[force_index] = placement; | ||
| } | ||
|
|
||
| } // namespace crocoddyl |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
|
There was a problem hiding this comment. This is the big change -- ForceDataAbstract is no longer a base class, it is something stored in a vector in InteractionDataAbstract, so each one can represent a force. A lot of common contact and impulse fields are pulled up in here |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We now have vectors for frame ids and placements, but I kept to just one type, else it caused a mess