Introduced floating base propeller actuation model

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

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>("ThrusterType")
+      .value("CW", CW)
+      .value("CCW", CCW)
+      .export_values();
+
+  bp::class_<Thruster>(
+      "Thruster", "Model for thrusters",
+      bp::init<pinocchio::SE3, double,
+               bp::optional<ThrusterType, double, double>>(
+          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<double, bp::optional<ThrusterType, double, double>>(
+          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<Thruster>())
+      .def(CopyableVisitor<Thruster>());
+
+  StdVectorPythonVisitor<std::vector<Thruster>, true>::expose(
+      "StdVec_Thruster");
+
+  bp::register_ptr_to_python<
+      boost::shared_ptr<crocoddyl::ActuationModelFloatingBaseThrusters>>();
+
+  bp::class_<ActuationModelFloatingBaseThrusters,
+             bp::bases<ActuationModelAbstract>>(
+      "ActuationModelFloatingBaseThrusters",
+      "Actuation models for floating base systems actuated with thrusters "
+      "(e.g. aerial "
+      "manipulators).",
+      bp::init<boost::shared_ptr<StateMultibody>, std::vector<Thruster>>(
+          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<void (ActuationModelFloatingBaseThrusters::*)(
+          const boost::shared_ptr<ActuationDataAbstract>&,
+          const Eigen::Ref<const Eigen::VectorXd>&,
+          const Eigen::Ref<const Eigen::VectorXd>&)>(
+          "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::return_by_value>()),
+          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<bp::return_by_value>()),
+          "matrix mapping from thrusts to thruster wrenches")
+      .add_property(
+          "S",
+          bp::make_function(&ActuationModelFloatingBaseThrusters::get_S,
+                            bp::return_value_policy<bp::return_by_value>()),
+          "selection matrix for under-actuation part")
+      .def(PrintableVisitor<ActuationModelFloatingBaseThrusters>())
+      .def(CopyableVisitor<ActuationModelFloatingBaseThrusters>());
+}
+
+}  // namespace python
+}  // namespace crocoddyl

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();

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

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)

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)

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)

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<const VectorXs>& x,
                         const Eigen::Ref<const VectorXs>& tau);
 
+  /**
+   * @brief @copydoc Base::torqueTransform()
+   */
+  virtual void torqueTransform(
+      const boost::shared_ptr<ActuationDataAbstract>& data,
+      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
+
   /**
    * @brief @copydoc Base::createData()
    */

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<Scalar>::commands(
                         std::to_string(model_->get_state()->get_nv()) + ")");
   }
   Data* d = static_cast<Data*>(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 <typename Scalar>
+void ActuationModelNumDiffTpl<Scalar>::torqueTransform(
+    const boost::shared_ptr<ActuationDataAbstract>& data,
+    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
+  if (static_cast<std::size_t>(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<std::size_t>(u.size()) != nu_) {
+    throw_pretty("Invalid argument: "
+                 << "u has wrong dimension (it should be " +
+                        std::to_string(nu_) + ")");
+  }
+  Data* d = static_cast<Data*>(data.get());
+  model_->torqueTransform(d->data_0, x, u);
+  d->Mtau = d->data_0->Mtau;
 }
 
 template <typename Scalar>