diff --git a/CHANGELOG.md b/CHANGELOG.md index 346da12416..445fdce126 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +* Introduced floating base thruster actuation model in https://github.com/loco-3d/crocoddyl/pull/1213 +* Fixed quadruped and biped examples in https://github.com/loco-3d/crocoddyl/pull/1208 +* Fixed terminal computation in Python models in https://github.com/loco-3d/crocoddyl/pull/1204 * Fixed handling of unbounded values for `ActivationBounds` in https://github.com/loco-3d/crocoddyl/pull/1191 ## [2.0.2] - 2023-12-07 diff --git a/bindings/python/crocoddyl/multibody/actuations/floating-base-propellers.cpp b/bindings/python/crocoddyl/multibody/actuations/floating-base-propellers.cpp new file mode 100644 index 0000000000..af39c3e264 --- /dev/null +++ b/bindings/python/crocoddyl/multibody/actuations/floating-base-propellers.cpp @@ -0,0 +1,141 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2024-2024, Heriot-Watt University +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#include "crocoddyl/multibody/actuations/floating-base-thrusters.hpp" +#include "python/crocoddyl/multibody/multibody.hpp" +#include "python/crocoddyl/utils/copyable.hpp" +#include "python/crocoddyl/utils/printable.hpp" +#include "python/crocoddyl/utils/vector-converter.hpp" + +namespace crocoddyl { + +namespace python { + +void exposeActuationFloatingBaseThruster() { + bp::enum_("ThrusterType") + .value("CW", CW) + .value("CCW", CCW) + .export_values(); + + bp::class_( + "Thruster", "Model for thrusters", + bp::init>( + bp::args("self", "M", "ctorque", "type", "min_thrust", "max_thrust"), + "Initialize the thruster in a give pose from the root joint.\n\n" + ":param M: pose from root joint\n" + ":param ctorque: coefficient of generated torque per thrust\n" + ":param type: type of thruster (clockwise or counterclockwise, " + "default clockwise)\n" + ":param min_thrust: minimum thrust (default 0.)\n" + ":param max_thrust: maximum thrust (default np.inf)")) + .def(bp::init>( + bp::args("self", "ctorque", "type", "min_thrust", "max_thrust"), + "Initialize the thruster in a give pose from the root joint.\n\n" + ":param ctorque: coefficient of generated torque per thrust\n" + ":param type: type of thruster (clockwise or counterclockwise, " + "default clockwise)\n" + ":param min_thrust: minimum thrust (default 0.)\n" + ":param max_thrust: maximum thrust (default np.inf)")) + .def_readwrite("pose", &Thruster::pose, + "thruster pose (traslation, rotation)") + .def_readwrite("ctorque", &Thruster::ctorque, + "coefficient of generated torque per thrust") + .def_readwrite("type", &Thruster::type, + "type of thruster (clockwise or counterclockwise)") + .def_readwrite("min_thrust", &Thruster::min_thrust, "minimum thrust") + .def_readwrite("max_thrust", &Thruster::min_thrust, "maximum thrust") + .def(PrintableVisitor()) + .def(CopyableVisitor()); + + StdVectorPythonVisitor, true>::expose( + "StdVec_Thruster"); + + bp::register_ptr_to_python< + boost::shared_ptr>(); + + bp::class_>( + "ActuationModelFloatingBaseThrusters", + "Actuation models for floating base systems actuated with thrusters " + "(e.g. aerial " + "manipulators).", + bp::init, std::vector>( + bp::args("self", "state", "thrusters"), + "Initialize the floating base actuation model equipped with " + "thrusters.\n\n" + ":param state: state of multibody system\n" + ":param thrusters: vector of thrusters")) + .def&, + const Eigen::Ref&, + const Eigen::Ref&)>( + "calc", &ActuationModelFloatingBaseThrusters::calc, + bp::args("self", "data", "x", "u"), + "Compute the actuation signal and actuation set from the thrust\n" + "forces and joint torque inputs u.\n\n" + ":param data: floating base thrusters actuation data\n" + ":param x: state point (dim. state.nx)\n" + ":param u: joint torque input (dim. nu)") + .def("calcDiff", &ActuationModelFloatingBaseThrusters::calcDiff, + bp::args("self", "data", "x", "u"), + "Compute the derivatives of the actuation model.\n\n" + "It computes the partial derivatives of the thruster actuation. It\n" + "assumes that calc has been run first. The reason is that the\n" + "derivatives are constant and defined in createData. The Hessian\n" + "is constant, so we don't write again this value.\n" + ":param data: floating base thrusters actuation data\n" + ":param x: state point (dim. state.nx)\n" + ":param u: joint torque input (dim. nu)") + .def("commands", &ActuationModelFloatingBaseThrusters::commands, + bp::args("self", "data", "x", "tau"), + "Compute the thrust and joint torque commands from the generalized " + "torques.\n\n" + "It stores the results in data.u.\n" + ":param data: actuation data\n" + ":param x: state point (dim. state.nx)\n" + ":param tau: generalized torques (dim state.nv)") + .def("torqueTransform", + &ActuationModelFloatingBaseThrusters::torqueTransform, + bp::args("self", "data", "x", "tau"), + "Compute the torque transform from generalized torques to thrust " + "and joint torque inputs.\n\n" + "It stores the results in data.Mtau.\n" + ":param data: floating base thrusters actuation data\n" + ":param x: state point (dim. state.nx)\n" + ":param tau: generalized torques (dim state.nv)") + .def("createData", &ActuationModelFloatingBaseThrusters::createData, + bp::args("self"), + "Create the floating base thrusters actuation data.") + .add_property( + "thrusters", + bp::make_function(&ActuationModelFloatingBaseThrusters::get_thrusters, + bp::return_value_policy()), + bp::make_function( + &ActuationModelFloatingBaseThrusters::set_thrusters), + "vector of thrusters") + .add_property("nthrusters", + bp::make_function( + &ActuationModelFloatingBaseThrusters::get_nthrusters), + "number of thrusters") + .add_property( + "Wthrust", + bp::make_function(&ActuationModelFloatingBaseThrusters::get_Wthrust, + bp::return_value_policy()), + "matrix mapping from thrusts to thruster wrenches") + .add_property( + "S", + bp::make_function(&ActuationModelFloatingBaseThrusters::get_S, + bp::return_value_policy()), + "selection matrix for under-actuation part") + .def(PrintableVisitor()) + .def(CopyableVisitor()); +} + +} // namespace python +} // namespace crocoddyl diff --git a/bindings/python/crocoddyl/multibody/multibody.cpp b/bindings/python/crocoddyl/multibody/multibody.cpp index 380b57745f..57dcfca1c7 100644 --- a/bindings/python/crocoddyl/multibody/multibody.cpp +++ b/bindings/python/crocoddyl/multibody/multibody.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University +// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -18,6 +18,7 @@ void exposeMultibody() { exposeStateMultibody(); exposeActuationFloatingBase(); exposeActuationFull(); + exposeActuationFloatingBaseThruster(); exposeActuationModelMultiCopterBase(); exposeForceAbstract(); exposeContactAbstract(); diff --git a/bindings/python/crocoddyl/multibody/multibody.hpp b/bindings/python/crocoddyl/multibody/multibody.hpp index 11c995f57b..4eb73b0a1b 100644 --- a/bindings/python/crocoddyl/multibody/multibody.hpp +++ b/bindings/python/crocoddyl/multibody/multibody.hpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh +// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh // Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. @@ -23,6 +23,7 @@ void exposeCoPSupport(); void exposeStateMultibody(); void exposeActuationFloatingBase(); void exposeActuationFull(); +void exposeActuationFloatingBaseThruster(); void exposeActuationModelMultiCopterBase(); void exposeForceAbstract(); void exposeContactAbstract(); @@ -62,7 +63,6 @@ void exposeContact3D(); void exposeContact6D(); void exposeImpulse3D(); void exposeImpulse6D(); - void exposeMultibody(); } // namespace python diff --git a/examples/quadrotor_fwddyn.py b/examples/quadrotor_fwddyn.py index 9d73cc2922..4ffdf574b9 100644 --- a/examples/quadrotor_fwddyn.py +++ b/examples/quadrotor_fwddyn.py @@ -22,17 +22,29 @@ state = crocoddyl.StateMultibody(robot_model) d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5.0, 0.1 -tau_f = np.array( - [ - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [1.0, 1.0, 1.0, 1.0], - [0.0, d_cog, 0.0, -d_cog], - [-d_cog, 0.0, d_cog, 0.0], - [-cm / cf, cm / cf, -cm / cf, cm / cf], - ] -) -actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f) +ps = [ + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([-d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, -d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), +] +actuation = crocoddyl.ActuationModelFloatingBaseThrusters(state, ps) nu = actuation.nu runningCostModel = crocoddyl.CostModelSum(state, nu) diff --git a/examples/quadrotor_invdyn.py b/examples/quadrotor_invdyn.py index 03ccedd4f8..f42f0cd7cc 100644 --- a/examples/quadrotor_invdyn.py +++ b/examples/quadrotor_invdyn.py @@ -22,17 +22,29 @@ state = crocoddyl.StateMultibody(robot_model) d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5.0, 0.1 -tau_f = np.array( - [ - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [1.0, 1.0, 1.0, 1.0], - [0.0, d_cog, 0.0, -d_cog], - [-d_cog, 0.0, d_cog, 0.0], - [-cm / cf, cm / cf, -cm / cf, cm / cf], - ] -) -actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f) +ps = [ + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([-d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, -d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), +] +actuation = crocoddyl.ActuationModelFloatingBaseThrusters(state, ps) nu = state.nv runningCostModel = crocoddyl.CostModelSum(state, nu) diff --git a/examples/quadrotor_ubound.py b/examples/quadrotor_ubound.py index 9865d51b62..ff1f656ce7 100644 --- a/examples/quadrotor_ubound.py +++ b/examples/quadrotor_ubound.py @@ -22,17 +22,29 @@ state = crocoddyl.StateMultibody(robot_model) d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5.0, 0.1 -tau_f = np.array( - [ - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [1.0, 1.0, 1.0, 1.0], - [0.0, d_cog, 0.0, -d_cog], - [-d_cog, 0.0, d_cog, 0.0], - [-cm / cf, cm / cf, -cm / cf, cm / cf], - ] -) -actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f) +ps = [ + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([-d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, -d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), +] +actuation = crocoddyl.ActuationModelFloatingBaseThrusters(state, ps) nu = actuation.nu runningCostModel = crocoddyl.CostModelSum(state, nu) diff --git a/include/crocoddyl/core/numdiff/actuation.hpp b/include/crocoddyl/core/numdiff/actuation.hpp index ef7115a6f0..b387341a40 100644 --- a/include/crocoddyl/core/numdiff/actuation.hpp +++ b/include/crocoddyl/core/numdiff/actuation.hpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, University of Edinburgh, LAAS-CNRS +// Copyright (C) 2019-2024, University of Edinburgh, LAAS-CNRS // Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. @@ -90,6 +90,13 @@ class ActuationModelNumDiffTpl : public ActuationModelAbstractTpl<_Scalar> { const Eigen::Ref& x, const Eigen::Ref& tau); + /** + * @brief @copydoc Base::torqueTransform() + */ + virtual void torqueTransform( + const boost::shared_ptr& data, + const Eigen::Ref& x, const Eigen::Ref& u); + /** * @brief @copydoc Base::createData() */ diff --git a/include/crocoddyl/core/numdiff/actuation.hxx b/include/crocoddyl/core/numdiff/actuation.hxx index 1c665f8f62..5a2e7f5cd1 100644 --- a/include/crocoddyl/core/numdiff/actuation.hxx +++ b/include/crocoddyl/core/numdiff/actuation.hxx @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, University of Edinburgh, LAAS-CNRS +// Copyright (C) 2019-2024, University of Edinburgh, LAAS-CNRS // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -138,9 +138,27 @@ void ActuationModelNumDiffTpl::commands( std::to_string(model_->get_state()->get_nv()) + ")"); } Data* d = static_cast(data.get()); + model_->commands(d->data_0, x, tau); + data->u = d->data_0->u; +} - model_->torqueTransform(d->data_x[0], x, tau); - data->u.noalias() = d->data_x[0]->Mtau * tau; +template +void ActuationModelNumDiffTpl::torqueTransform( + const boost::shared_ptr& data, + const Eigen::Ref& x, const Eigen::Ref& u) { + if (static_cast(x.size()) != model_->get_state()->get_nx()) { + throw_pretty("Invalid argument: " + << "x has wrong dimension (it should be " + + std::to_string(model_->get_state()->get_nx()) + ")"); + } + if (static_cast(u.size()) != nu_) { + throw_pretty("Invalid argument: " + << "u has wrong dimension (it should be " + + std::to_string(nu_) + ")"); + } + Data* d = static_cast(data.get()); + model_->torqueTransform(d->data_0, x, u); + d->Mtau = d->data_0->Mtau; } template diff --git a/include/crocoddyl/multibody/actuations/floating-base-thrusters.hpp b/include/crocoddyl/multibody/actuations/floating-base-thrusters.hpp new file mode 100644 index 0000000000..09631df577 --- /dev/null +++ b/include/crocoddyl/multibody/actuations/floating-base-thrusters.hpp @@ -0,0 +1,352 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2014-2024, Heriot-Watt University +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_ +#define CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_ + +#include +#include +#include + +#include "crocoddyl/core/actuation-base.hpp" +#include "crocoddyl/core/utils/exception.hpp" +#include "crocoddyl/multibody/states/multibody.hpp" + +namespace crocoddyl { + +enum ThrusterType { CW = 0, CCW }; + +template +struct ThrusterTpl { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef _Scalar Scalar; + typedef pinocchio::SE3Tpl SE3; + typedef ThrusterTpl Thruster; + + /** + * @brief Initialize the thruster in a give pose from the root joint. + * + * @param pose[in] Pose from root joint + * @param ctorque[in] Coefficient of generated torque per thrust + * @param type[in] Type of thruster (clockwise or counterclockwise, + * default clockwise) + * @param[in] min_thrust[in] Minimum thrust (default 0.) + * @param[in] max_thrust[in] Maximum thrust (default inf number)) + */ + ThrusterTpl(const SE3& pose, const Scalar ctorque, + const ThrusterType type = CW, + const Scalar min_thrust = Scalar(0.), + const Scalar max_thrust = std::numeric_limits::infinity()) + : pose(pose), + ctorque(ctorque), + type(type), + min_thrust(min_thrust), + max_thrust(max_thrust) {} + + /** + * @brief Initialize the thruster in a pose in the origin of the root joint. + * + * @param pose[in] Pose from root joint + * @param ctorque[in] Coefficient of generated torque per thrust + * @param type[in] Type of thruster (clockwise or counterclockwise, + * default clockwise) + * @param[in] min_thrust[in] Minimum thrust (default 0.) + * @param[in] max_thrust[in] Maximum thrust (default inf number)) + */ + ThrusterTpl(const Scalar ctorque, const ThrusterType type = CW, + const Scalar min_thrust = Scalar(0.), + const Scalar max_thrust = std::numeric_limits::infinity()) + : pose(SE3::Identity()), + ctorque(ctorque), + type(type), + min_thrust(min_thrust), + max_thrust(max_thrust) {} + ThrusterTpl(const ThrusterTpl& clone) + : pose(clone.pose), + ctorque(clone.ctorque), + type(clone.type), + min_thrust(clone.min_thrust), + max_thrust(clone.max_thrust) {} + + ThrusterTpl& operator=(const ThrusterTpl& other) { + if (this != &other) { + pose = other.pose; + ctorque = other.ctorque; + type = other.type; + min_thrust = other.min_thrust; + max_thrust = other.max_thrust; + } + return *this; + } + + template + bool operator==(const ThrusterTpl& other) const { + return (pose == other.pose && ctorque == other.ctorque && + type == other.type && min_thrust == other.min_thrust && + max_thrust == other.max_thrust); + } + + friend std::ostream& operator<<(std::ostream& os, + const ThrusterTpl& X) { + os << " pose:" << std::endl + << X.pose << " ctorque: " << X.ctorque << std::endl + << " type: " << X.type << std::endl + << "min_thrust: " << X.min_thrust << std::endl + << "max_thrust: " << X.max_thrust << std::endl; + return os; + } + + SE3 pose; //!< Thruster pose + Scalar ctorque; //!< Coefficient of generated torque per thrust + ThrusterType type; //!< Type of thruster (CW and CCW for clockwise and + //!< counterclockwise, respectively) + Scalar min_thrust; //!< Minimum thrust + Scalar max_thrust; //!< Minimum thrust +}; + +/** + * @brief Actuation models for floating base systems actuated with thrusters + * + * This actuation model models floating base robots equipped with thrusters, + * e.g., multicopters or marine robots equipped with manipulators. It control + * inputs are the thrusters' thrust (i.e., forces) and joint torques. + * + * Both actuation and Jacobians are computed analytically by `calc` and + * `calcDiff`, respectively. + * + * We assume the robot velocity to zero for easily related square thruster + * velocities with thrust and torque generated. This approach is similarly + * implemented in M. Geisert and N. Mansard, "Trajectory generation for + * quadrotor based systems using numerical optimal control", (ICRA). See Section + * III.C. + * + * \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` + */ +template +class ActuationModelFloatingBaseThrustersTpl + : public ActuationModelAbstractTpl<_Scalar> { + public: + typedef _Scalar Scalar; + typedef MathBaseTpl MathBase; + typedef ActuationModelAbstractTpl Base; + typedef ActuationDataAbstractTpl Data; + typedef StateMultibodyTpl StateMultibody; + typedef ThrusterTpl Thruster; + typedef typename MathBase::Vector3s Vector3s; + typedef typename MathBase::VectorXs VectorXs; + typedef typename MathBase::MatrixXs MatrixXs; + + /** + * @brief Initialize the floating base actuation model equipped with + * thrusters + * + * @param[in] state State of the dynamical system + * @param[in] thrusters Vector of thrusters + */ + ActuationModelFloatingBaseThrustersTpl( + boost::shared_ptr state, + const std::vector& thrusters) + : Base(state, + state->get_nv() - + state->get_pinocchio() + ->joints[( + state->get_pinocchio()->existJointName("root_joint") + ? state->get_pinocchio()->getJointId("root_joint") + : 0)] + .nv() + + thrusters.size()), + thrusters_(thrusters), + n_thrusters_(thrusters.size()), + W_thrust_(state_->get_nv(), nu_), + update_data_(true) { + if (!state->get_pinocchio()->existJointName("root_joint")) { + throw_pretty( + "Invalid argument: " + << "the first joint has to be a root one (e.g., free-flyer joint)"); + } + // Update the joint actuation part + W_thrust_.setZero(); + if (nu_ > n_thrusters_) { + W_thrust_ + .template bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_) + .diagonal() + .setOnes(); + } + // Update the floating base actuation part + set_thrusters(thrusters_); + } + virtual ~ActuationModelFloatingBaseThrustersTpl() {} + + /** + * @brief Compute the actuation signal and actuation set from its thrust + * and joint torque inputs \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ + * + * @param[in] data Floating base thrusters actuation data + * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ + * @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ + */ + virtual void calc(const boost::shared_ptr& data, + const Eigen::Ref&, + const Eigen::Ref& u) { + if (static_cast(u.size()) != nu_) { + throw_pretty("Invalid argument: " + << "u has wrong dimension (it should be " + + std::to_string(nu_) + ")"); + } + if (update_data_) { + updateData(data); + } + data->tau.noalias() = data->dtau_du * u; + } + + /** + * @brief Compute the Jacobians of the floating base thruster actuation + * function + * + * @param[in] data Floating base thrusters actuation data + * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ + * @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ + */ +#ifndef NDEBUG + virtual void calcDiff(const boost::shared_ptr& data, + const Eigen::Ref&, + const Eigen::Ref&) { +#else + virtual void calcDiff(const boost::shared_ptr&, + const Eigen::Ref&, + const Eigen::Ref&) { +#endif + // The derivatives has constant values which were set in createData. + assert_pretty(MatrixXs(data->dtau_du).isApprox(W_thrust_), + "dtau_du has wrong value"); + } + + virtual void commands(const boost::shared_ptr& data, + const Eigen::Ref&, + const Eigen::Ref& tau) { + data->u.noalias() = data->Mtau * tau; + } + +#ifndef NDEBUG + virtual void torqueTransform(const boost::shared_ptr& data, + const Eigen::Ref&, + const Eigen::Ref&) { +#else + virtual void torqueTransform(const boost::shared_ptr&, + const Eigen::Ref&, + const Eigen::Ref&) { +#endif + // The torque transform has constant values which were set in createData. + assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value"); + } + + /** + * @brief Create the floating base thruster actuation data + * + * @return the actuation data + */ + virtual boost::shared_ptr createData() { + boost::shared_ptr data = + boost::allocate_shared(Eigen::aligned_allocator(), this); + updateData(data); + return data; + } + + /** + * @brief Return the vector of thrusters + */ + const std::vector& get_thrusters() const { return thrusters_; } + + /** + * @brief Return the number of thrusters + */ + std::size_t get_nthrusters() const { return n_thrusters_; } + + /** + * @brief Modify the vector of thrusters + * + * Since we don't want to allocate data, we request to pass the same + * number of thrusters. + * + * @param[in] thrusters Vector of thrusters + */ + void set_thrusters(const std::vector& thrusters) { + if (static_cast(thrusters.size()) != n_thrusters_) { + throw_pretty("Invalid argument: " + << "the number of thrusters is wrong (it should be " + + std::to_string(n_thrusters_) + ")"); + } + thrusters_ = thrusters; + // Update the mapping matrix from thrusters thrust to body force/moments + for (std::size_t i = 0; i < n_thrusters_; ++i) { + const Thruster& p = thrusters_[i]; + const Vector3s& f_z = p.pose.rotation() * Vector3s::UnitZ(); + W_thrust_.template topRows<3>().col(i) += f_z; + W_thrust_.template middleRows<3>(3).col(i).noalias() += + p.pose.translation().cross(Vector3s::UnitZ()); + switch (p.type) { + case CW: + W_thrust_.template middleRows<3>(3).col(i) += p.ctorque * f_z; + break; + case CCW: + W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z; + break; + } + } + // Compute the torque transform matrix from generalized torques to joint + // torque inputs + Mtau_ = pseudoInverse(MatrixXs(W_thrust_)); + S_.noalias() = W_thrust_ * Mtau_; + update_data_ = true; + } + + const MatrixXs& get_Wthrust() const { return W_thrust_; } + + const MatrixXs& get_S() const { return S_; } + + void print(std::ostream& os) const { + os << "ActuationModelFloatingBaseThrusters {nu=" << nu_ + << ", nthrusters=" << n_thrusters_ << ", thrusters=" << std::endl; + for (std::size_t i = 0; i < n_thrusters_; ++i) { + os << std::to_string(i) << ": " << thrusters_[i]; + } + os << "}"; + } + + protected: + std::vector thrusters_; //!< Vector of thrusters + std::size_t n_thrusters_; //!< Number of thrusters + MatrixXs W_thrust_; //!< Matrix from thrusters thrusts to body wrench + MatrixXs Mtau_; //!< Constaint torque transform from generalized torques to + //!< joint torque inputs + MatrixXs S_; //!< Selection matrix for under-actuation part + + bool update_data_; + using Base::nu_; + using Base::state_; + + private: + void updateData(const boost::shared_ptr& data) { + data->dtau_du = W_thrust_; + data->Mtau = Mtau_; + const std::size_t nv = state_->get_nv(); + for (std::size_t k = 0; k < nv; ++k) { + if (fabs(S_(k, k)) < std::numeric_limits::epsilon()) { + data->tau_set[k] = false; + } else { + data->tau_set[k] = true; + } + } + update_data_ = false; + } +}; + +} // namespace crocoddyl + +#endif // CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_ diff --git a/include/crocoddyl/multibody/actuations/multicopter-base.hpp b/include/crocoddyl/multibody/actuations/multicopter-base.hpp index b752a88c51..7954f65092 100644 --- a/include/crocoddyl/multibody/actuations/multicopter-base.hpp +++ b/include/crocoddyl/multibody/actuations/multicopter-base.hpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2022, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh +// Copyright (C) 2019-2024, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -59,25 +59,10 @@ class ActuationModelMultiCopterBaseTpl * @param[in] tau_f Matrix that maps the thrust of each propeller to the net * force and torque */ - ActuationModelMultiCopterBaseTpl(boost::shared_ptr state, - const Eigen::Ref& tau_f) - : Base(state, state->get_nv() - 6 + tau_f.cols()), - n_rotors_(tau_f.cols()) { - pinocchio::JointModelFreeFlyerTpl ff_joint; - if (state->get_pinocchio()->joints[1].shortname() != ff_joint.shortname()) { - throw_pretty("Invalid argument: " - << "the first joint has to be free-flyer"); - } - - tau_f_ = MatrixXs::Zero(state_->get_nv(), nu_); - tau_f_.block(0, 0, 6, n_rotors_) = tau_f; - if (nu_ > n_rotors_) { - tau_f_.bottomRightCorner(nu_ - n_rotors_, nu_ - n_rotors_) - .diagonal() - .setOnes(); - } - Mtau_ = pseudoInverse(MatrixXs(tau_f)); - } + DEPRECATED("Use constructor ActuationModelFloatingBaseThrustersTpl", + ActuationModelMultiCopterBaseTpl( + boost::shared_ptr state, + const Eigen::Ref& tau_f)); DEPRECATED("Use constructor without n_rotors", ActuationModelMultiCopterBaseTpl( @@ -161,6 +146,30 @@ class ActuationModelMultiCopterBaseTpl #endif }; +template +ActuationModelMultiCopterBaseTpl::ActuationModelMultiCopterBaseTpl( + boost::shared_ptr state, + const Eigen::Ref& tau_f) + : Base(state, state->get_nv() - 6 + tau_f.cols()), n_rotors_(tau_f.cols()) { + pinocchio::JointModelFreeFlyerTpl ff_joint; + if (state->get_pinocchio()->joints[1].shortname() != ff_joint.shortname()) { + throw_pretty("Invalid argument: " + << "the first joint has to be free-flyer"); + } + + tau_f_ = MatrixXs::Zero(state_->get_nv(), nu_); + tau_f_.block(0, 0, 6, n_rotors_) = tau_f; + if (nu_ > n_rotors_) { + tau_f_.bottomRightCorner(nu_ - n_rotors_, nu_ - n_rotors_) + .diagonal() + .setOnes(); + } + Mtau_ = pseudoInverse(MatrixXs(tau_f)); + std::cerr << "Deprecated ActuationModelMultiCopterBase: Use " + "ActuationModelFloatingBaseThrusters" + << std::endl; +} + template ActuationModelMultiCopterBaseTpl::ActuationModelMultiCopterBaseTpl( boost::shared_ptr state, const std::size_t n_rotors, diff --git a/include/crocoddyl/multibody/fwd.hpp b/include/crocoddyl/multibody/fwd.hpp index d404b21310..c6e44428d8 100644 --- a/include/crocoddyl/multibody/fwd.hpp +++ b/include/crocoddyl/multibody/fwd.hpp @@ -1,7 +1,8 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh, INRIA +// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh, INRIA +// Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -21,6 +22,12 @@ class ActuationModelFloatingBaseTpl; template class ActuationModelFullTpl; +template +struct ThrusterTpl; + +template +class ActuationModelFloatingBaseThrustersTpl; + template class ActuationModelMultiCopterBaseTpl; @@ -244,12 +251,16 @@ class ImpulseModelMultipleTpl; template struct ImpulseDataMultipleTpl; -/*******************************Template - * Instantiation**************************/ +/**** Template Instantiation ****/ typedef ActuationModelFloatingBaseTpl ActuationModelFloatingBase; typedef ActuationModelFullTpl ActuationModelFull; -typedef ActuationModelMultiCopterBaseTpl ActuationModelMultiCopterBase; +typedef ThrusterTpl Thruster; +typedef ActuationModelFloatingBaseThrustersTpl + ActuationModelFloatingBaseThrusters; +DEPRECATED("Use ActuationModelFloatingBaseThrustersTpl", + typedef ActuationModelMultiCopterBaseTpl + ActuationModelMultiCopterBase;) typedef ForceDataAbstractTpl ForceDataAbstract; diff --git a/unittest/bindings/test_copy.py b/unittest/bindings/test_copy.py index 126cfe2e1b..aae46367f5 100644 --- a/unittest/bindings/test_copy.py +++ b/unittest/bindings/test_copy.py @@ -302,7 +302,31 @@ class ActuationsTest(CopyModelTestCase): # multibody actuations MODEL.append(crocoddyl.ActuationModelFloatingBase(state)) MODEL.append(crocoddyl.ActuationModelFull(state)) - MODEL.append(crocoddyl.ActuationModelMultiCopterBase(state, np.ones(6))) + d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5.0, 0.1 + ps = [ + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([-d_cog, 0, 0])), + cm / cf, + crocoddyl.ThrusterType.CCW, + ), + crocoddyl.Thruster( + pinocchio.SE3(np.eye(3), np.array([0, -d_cog, 0])), + cm / cf, + crocoddyl.ThrusterType.CW, + ), + ] + actuation = crocoddyl.ActuationModelFloatingBaseThrusters(state, ps) + MODEL.append(crocoddyl.ActuationModelFloatingBaseThrusters(state, ps)) class ContactsTest(CopyModelTestCase): diff --git a/unittest/factory/actuation.cpp b/unittest/factory/actuation.cpp index de8886e0d5..8768ae0199 100644 --- a/unittest/factory/actuation.cpp +++ b/unittest/factory/actuation.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2021, University of Edinburgh +// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -12,9 +12,9 @@ #include "crocoddyl/core/actuation/squashing-base.hpp" #include "crocoddyl/core/actuation/squashing/smooth-sat.hpp" #include "crocoddyl/core/utils/exception.hpp" +#include "crocoddyl/multibody/actuations/floating-base-thrusters.hpp" #include "crocoddyl/multibody/actuations/floating-base.hpp" #include "crocoddyl/multibody/actuations/full.hpp" -#include "crocoddyl/multibody/actuations/multicopter-base.hpp" namespace crocoddyl { namespace unittest { @@ -30,8 +30,8 @@ std::ostream& operator<<(std::ostream& os, ActuationModelTypes::Type type) { case ActuationModelTypes::ActuationModelFloatingBase: os << "ActuationModelFloatingBase"; break; - case ActuationModelTypes::ActuationModelMultiCopterBase: - os << "ActuationModelMultiCopterBase"; + case ActuationModelTypes::ActuationModelFloatingBaseThrusters: + os << "ActuationModelFloatingBaseThrusters"; break; case ActuationModelTypes::ActuationModelSquashingFull: os << "ActuationModelSquashingFull"; @@ -56,8 +56,19 @@ ActuationModelFactory::create(ActuationModelTypes::Type actuation_type, boost::shared_ptr state = factory.create(state_type); boost::shared_ptr state_multibody; - // MultiCopter objects - Eigen::MatrixXd tau_f; + // Thruster objects + std::vector ps; + const double d_cog = 0.1525; + const double cf = 6.6e-5; + const double cm = 1e-6; + pinocchio::SE3 p1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(d_cog, 0, 0)); + pinocchio::SE3 p2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, d_cog, 0)); + pinocchio::SE3 p3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-d_cog, 0, 0)); + pinocchio::SE3 p4(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, -d_cog, 0)); + ps.push_back(crocoddyl::Thruster(p1, cm / cf, crocoddyl::ThrusterType::CCW)); + ps.push_back(crocoddyl::Thruster(p2, cm / cf, crocoddyl::ThrusterType::CW)); + ps.push_back(crocoddyl::Thruster(p3, cm / cf, crocoddyl::ThrusterType::CW)); + ps.push_back(crocoddyl::Thruster(p4, cm / cf, crocoddyl::ThrusterType::CCW)); // Actuation Squashing objects boost::shared_ptr act; boost::shared_ptr squash; @@ -76,16 +87,12 @@ ActuationModelFactory::create(ActuationModelTypes::Type actuation_type, actuation = boost::make_shared( state_multibody); break; - case ActuationModelTypes::ActuationModelMultiCopterBase: + case ActuationModelTypes::ActuationModelFloatingBaseThrusters: state_multibody = boost::static_pointer_cast(state); - tau_f = Eigen::MatrixXd::Zero(6, 4); - tau_f.row(2).fill(1.0); - tau_f.row(3) << 0.0, 0.1525, 0.0, -0.1525; - tau_f.row(4) << -0.1525, 0.0, 0.1525, 0.0; - tau_f.row(5) << -0.01515, 0.01515, -0.01515, 0.01515; - actuation = boost::make_shared( - state_multibody, tau_f); + actuation = + boost::make_shared( + state_multibody, ps); break; case ActuationModelTypes::ActuationModelSquashingFull: state_multibody = diff --git a/unittest/factory/actuation.hpp b/unittest/factory/actuation.hpp index dd0a985cbc..456438e90e 100644 --- a/unittest/factory/actuation.hpp +++ b/unittest/factory/actuation.hpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2021, University of Edinburgh +// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -20,7 +20,7 @@ struct ActuationModelTypes { enum Type { ActuationModelFull, ActuationModelFloatingBase, - ActuationModelMultiCopterBase, + ActuationModelFloatingBaseThrusters, ActuationModelSquashingFull, NbActuationModelTypes }; diff --git a/unittest/factory/diff_action.cpp b/unittest/factory/diff_action.cpp index 85dc5bf3d1..fb8a788246 100644 --- a/unittest/factory/diff_action.cpp +++ b/unittest/factory/diff_action.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, University of Edinburgh, CTU, INRIA, +// Copyright (C) 2019-2024, University of Edinburgh, CTU, INRIA, // Heriot-Watt University, University of Pisa // Copyright note valid unless otherwise stated in individual files. // All rights reserved. @@ -151,7 +151,7 @@ DifferentialActionModelFactory::create(DifferentialActionModelTypes::Type type, DifferentialActionModelFreeFwdDynamics_Hector: action = create_freeFwdDynamics( StateModelTypes::StateMultibody_Hector, - ActuationModelTypes::ActuationModelMultiCopterBase, false); + ActuationModelTypes::ActuationModelFloatingBaseThrusters, false); break; case DifferentialActionModelTypes:: DifferentialActionModelFreeFwdDynamics_TalosArm: @@ -168,7 +168,7 @@ DifferentialActionModelFactory::create(DifferentialActionModelTypes::Type type, DifferentialActionModelFreeInvDynamics_Hector: action = create_freeInvDynamics( StateModelTypes::StateMultibody_Hector, - ActuationModelTypes::ActuationModelMultiCopterBase); + ActuationModelTypes::ActuationModelFloatingBaseThrusters); break; case DifferentialActionModelTypes:: DifferentialActionModelFreeInvDynamics_TalosArm: diff --git a/unittest/test_actuation.cpp b/unittest/test_actuation.cpp index 270dc10122..d6a682f3b5 100644 --- a/unittest/test_actuation.cpp +++ b/unittest/test_actuation.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University +// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -232,7 +232,7 @@ bool init_function() { } register_actuation_model_unit_tests( - ActuationModelTypes::ActuationModelMultiCopterBase, + ActuationModelTypes::ActuationModelFloatingBaseThrusters, StateModelTypes::StateMultibody_Hector); return true; } diff --git a/unittest/test_residuals.cpp b/unittest/test_residuals.cpp index 3564d87f24..80cf9652cd 100644 --- a/unittest/test_residuals.cpp +++ b/unittest/test_residuals.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2021-2023, University of Edinburgh, Heriot-Watt University +// Copyright (C) 2021-2024, University of Edinburgh, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -300,7 +300,7 @@ bool init_function() { for (size_t actuation_type = 0; actuation_type < ActuationModelTypes::all.size(); ++actuation_type) { if (ActuationModelTypes::all[actuation_type] != - ActuationModelTypes::ActuationModelMultiCopterBase) { + ActuationModelTypes::ActuationModelFloatingBaseThrusters) { register_residual_model_unit_tests( ResidualModelTypes::all[residual_type], StateModelTypes::all[state_type],