Merge pull request #1455 from cmastalli/topic/actuation-limits

Author: cmastalli

CHANGELOG.md

@@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+* Introduced the actuation limits to handle thrusters and general action models in https://github.com/loco-3d/crocoddyl/pull/1455
 * Introduced safe difference and integration functions for states in https://github.com/loco-3d/crocoddyl/pull/1448
 * ROS: jrl_cmakemodules dependency + kilted CI in https://github.com/loco-3d/crocoddyl/pull/1441
 

CMakeLists.txt

@@ -292,6 +292,12 @@ if(UNIX AND NOT BUILD_STANDALONE_PYTHON_INTERFACE)
     ${PROJECT_NAME} PUBLIC PINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_2)
   set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${PROJECT_VERSION})
 
+  if(BUILD_WITH_CODEGEN_SUPPORT)
+    # CppAD is header-only but ships cppad_lib for non-header utilities (e.g.
+    # temp_file)
+    target_link_libraries(${PROJECT_NAME} ${cppad_LIBRARY})
+  endif()
+
   if(BUILD_WITH_MULTITHREADS)
     target_link_libraries(${PROJECT_NAME} OpenMP::OpenMP_CXX)
   endif()

bindings/python/crocoddyl/core/actuation-base.cpp.in

@@ -97,7 +97,15 @@ struct ActuationModelAbstractVisitor
             "state",
             bp::make_function(&Model::get_state,
                               bp::return_value_policy<bp::return_by_value>()),
-            "state");
+            "state")
+        .add_property("u_lb",
+                      bp::make_function(&Model::get_u_lb,
+                                        bp::return_internal_reference<>()),
+                      &Model::set_u_lb, "lower actuation limits")
+        .add_property("u_ub",
+                      bp::make_function(&Model::get_u_ub,
+                                        bp::return_internal_reference<>()),
+                      &Model::set_u_ub, "upper actuation limits");
   }
 };
 

include/crocoddyl/core/actuation-base.hpp

@@ -155,6 +155,26 @@ class ActuationModelAbstractTpl : public ActuationModelBase {
    */
   const std::shared_ptr<StateAbstract>& get_state() const;
 
+  /**
+   * @brief Return the actuation lower bound
+   */
+  const VectorXs& get_u_lb() const;
+
+  /**
+   * @brief Return the actuation upper bound
+   */
+  const VectorXs& get_u_ub() const;
+
+  /**
+   * @brief Modify the actuation lower bounds
+   */
+  void set_u_lb(const VectorXs& u_lb);
+
+  /**
+   * @brief Modify the actuation upper bounds
+   */
+  void set_u_ub(const VectorXs& u_ub);
+
   /**
    * @brief Print information on the actuation model
    */
@@ -172,6 +192,8 @@ class ActuationModelAbstractTpl : public ActuationModelBase {
  protected:
   std::size_t nu_;                        //!< Dimension of joint torque inputs
   std::shared_ptr<StateAbstract> state_;  //!< Model of the state
+  VectorXs u_lb_;                         //!< Lower actuation limits
+  VectorXs u_ub_;                         //!< Upper actuation limits
   ActuationModelAbstractTpl() : nu_(0), state_(nullptr) {};
 };
 

include/crocoddyl/core/actuation-base.hxx

@@ -12,7 +12,12 @@ namespace crocoddyl {
 template <typename Scalar>
 ActuationModelAbstractTpl<Scalar>::ActuationModelAbstractTpl(
     std::shared_ptr<StateAbstract> state, const std::size_t nu)
-    : nu_(nu), state_(state) {}
+    : nu_(nu),
+      state_(state),
+      u_lb_(MathBase::VectorXs::Constant(
+          nu, -std::numeric_limits<Scalar>::infinity())),
+      u_ub_(MathBase::VectorXs::Constant(
+          nu, std::numeric_limits<Scalar>::infinity())) {}
 
 template <typename Scalar>
 std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
@@ -61,6 +66,48 @@ ActuationModelAbstractTpl<Scalar>::get_state() const {
   return state_;
 }
 
+template <typename Scalar>
+const typename MathBaseTpl<Scalar>::VectorXs&
+ActuationModelAbstractTpl<Scalar>::get_u_lb() const {
+  return u_lb_;
+}
+
+template <typename Scalar>
+const typename MathBaseTpl<Scalar>::VectorXs&
+ActuationModelAbstractTpl<Scalar>::get_u_ub() const {
+  return u_ub_;
+}
+
+template <typename Scalar>
+void ActuationModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
+  if (static_cast<std::size_t>(u_lb.size()) != nu_) {
+    throw_pretty("Invalid argument: "
+                 << "control lower bound has wrong dimension (it should be " +
+                        std::to_string(nu_) + ")");
+  }
+  u_lb_ = u_lb;
+  for (std::size_t i = 0; i < nu_; ++i) {
+    if (u_lb_[i] <= -std::numeric_limits<Scalar>::max()) {
+      u_lb_[i] = -std::numeric_limits<Scalar>::infinity();
+    }
+  }
+}
+
+template <typename Scalar>
+void ActuationModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
+  if (static_cast<std::size_t>(u_ub.size()) != nu_) {
+    throw_pretty("Invalid argument: "
+                 << "control upper bound has wrong dimension (it should be " +
+                        std::to_string(nu_) + ")");
+  }
+  u_ub_ = u_ub;
+  for (std::size_t i = 0; i < nu_; ++i) {
+    if (u_ub_[i] >= std::numeric_limits<Scalar>::max()) {
+      u_ub_[i] = std::numeric_limits<Scalar>::infinity();
+    }
+  }
+}
+
 template <typename Scalar>
 std::ostream& operator<<(std::ostream& os,
                          const ActuationModelAbstractTpl<Scalar>& model) {

include/crocoddyl/multibody/actions/contact-fwddyn.hxx

@@ -73,9 +73,8 @@ void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::init() {
         << "Costs doesn't have the same control dimension (it should be " +
                std::to_string(nu_) + ")");
   }
-
-  Base::set_u_lb(Scalar(-1.) * pinocchio_->effortLimit.tail(nu_));
-  Base::set_u_ub(Scalar(+1.) * pinocchio_->effortLimit.tail(nu_));
+  Base::u_lb_ = actuation_->get_u_lb();
+  Base::u_ub_ = actuation_->get_u_ub();
 }
 
 template <typename Scalar>

include/crocoddyl/multibody/actions/free-fwddyn.hxx

@@ -33,8 +33,8 @@ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::
         << "Costs doesn't have the same control dimension (it should be " +
                std::to_string(nu_) + ")");
   }
-  Base::set_u_lb(Scalar(-1.) * pinocchio_->effortLimit.tail(nu_));
-  Base::set_u_ub(Scalar(+1.) * pinocchio_->effortLimit.tail(nu_));
+  Base::u_lb_ = actuation_->get_u_lb();
+  Base::u_ub_ = actuation_->get_u_ub();
 }
 
 template <typename Scalar>

include/crocoddyl/multibody/actuations/floating-base-thrusters.hpp

@@ -178,11 +178,22 @@ class ActuationModelFloatingBaseThrustersTpl
           << "the first joint has to be a root one (e.g., free-flyer joint)");
     }
     // Update the joint actuation part
+    const std::size_t nv_f =
+        state->get_pinocchio()
+            ->joints[(state->get_pinocchio()->existJointName("root_joint")
+                          ? state->get_pinocchio()->getJointId("root_joint")
+                          : 0)]
+            .nv();
     W_thrust_.setZero();
     if (nu_ > n_thrusters_) {
       W_thrust_.bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_)
           .diagonal()
           .setOnes();
+      Base::u_lb_.tail(nu_ - n_thrusters_) =
+          Scalar(-1.) *
+          state->get_pinocchio()->effortLimit.segment(nv_f, nu_ - n_thrusters_);
+      Base::u_ub_.tail(nu_ - n_thrusters_) =
+          state->get_pinocchio()->effortLimit.segment(nv_f, nu_ - n_thrusters_);
     }
     // Update the floating base actuation part
     set_thrusters(thrusters_);
@@ -316,6 +327,8 @@ class ActuationModelFloatingBaseThrustersTpl
           W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z;
           break;
       }
+      Base::u_lb_(i) = p.min_thrust;
+      Base::u_ub_(i) = p.max_thrust;
     }
     // Compute the torque transform matrix from generalized torques to joint
     // torque inputs

include/crocoddyl/multibody/actuations/floating-base.hpp

@@ -60,7 +60,10 @@ class ActuationModelFloatingBaseTpl
                          state->get_pinocchio()->existJointName("root_joint")
                              ? state->get_pinocchio()->getJointId("root_joint")
                              : 0)]
-                     .nv()) {};
+                     .nv()) {
+    Base::u_lb_ = Scalar(-1.) * state->get_pinocchio()->effortLimit.tail(nu_);
+    Base::u_ub_ = state->get_pinocchio()->effortLimit.tail(nu_);
+  };
   virtual ~ActuationModelFloatingBaseTpl() = default;
 
   /**

include/crocoddyl/multibody/actuations/full.hpp

@@ -13,6 +13,7 @@
 #include "crocoddyl/core/actuation-base.hpp"
 #include "crocoddyl/core/state-base.hpp"
 #include "crocoddyl/multibody/fwd.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
 
 namespace crocoddyl {
 
@@ -47,7 +48,14 @@ class ActuationModelFullTpl : public ActuationModelAbstractTpl<_Scalar> {
    * @param[in] state  State of the dynamical system
    */
   explicit ActuationModelFullTpl(std::shared_ptr<StateAbstract> state)
-      : Base(state, state->get_nv()) {};
+      : Base(state, state->get_nv()) {
+    StateMultibodyTpl<Scalar>* s =
+        dynamic_cast<StateMultibodyTpl<Scalar>*>(state.get());
+    if (s != NULL) {
+      Base::u_lb_ = Scalar(-1.) * s->get_pinocchio()->effortLimit;
+      Base::u_ub_ = s->get_pinocchio()->effortLimit;
+    }
+  };
   virtual ~ActuationModelFullTpl() = default;
 
   /**