Merge pull request #1171 from cmastalli/topic/constraints-api

loco-3d · Oct 16, 2023 · 62d0eac · 62d0eac
2 parents e3cdd07 + 8175cb3
commit 62d0eac
Show file tree

Hide file tree

Showing 6 changed files with 91 additions and 23 deletions.
diff --git a/bindings/python/crocoddyl/core/action-base.cpp b/bindings/python/crocoddyl/core/action-base.cpp
@@ -132,15 +132,13 @@ void exposeActionAbstract() {
       .add_property("g_lb",
                     bp::make_function(&ActionModelAbstract_wrap::get_g_lb,
                                       bp::return_internal_reference<>()),
+                    &ActionModelAbstract_wrap::set_g_lb,
                     "lower bound of the inequality constraints")
       .add_property("g_ub",
                     bp::make_function(&ActionModelAbstract_wrap::get_g_ub,
                                       bp::return_internal_reference<>()),
+                    &ActionModelAbstract_wrap::set_g_ub,
                     "upper bound of the inequality constraints")
-      .add_property(
-          "has_control_limits",
-          bp::make_function(&ActionModelAbstract_wrap::get_has_control_limits),
-          "indicates whether problem has finite control limits")
       .add_property("u_lb",
                     bp::make_function(&ActionModelAbstract_wrap::get_u_lb,
                                       bp::return_internal_reference<>()),
@@ -149,6 +147,10 @@ void exposeActionAbstract() {
                     bp::make_function(&ActionModelAbstract_wrap::get_u_ub,
                                       bp::return_internal_reference<>()),
                     &ActionModelAbstract_wrap::set_u_ub, "upper control limits")
+      .add_property(
+          "has_control_limits",
+          bp::make_function(&ActionModelAbstract_wrap::get_has_control_limits),
+          "indicates whether problem has finite control limits")
       .def(PrintableVisitor<ActionModelAbstract>());
 
   bp::register_ptr_to_python<boost::shared_ptr<ActionDataAbstract> >();

diff --git a/bindings/python/crocoddyl/core/diff-action-base.cpp b/bindings/python/crocoddyl/core/diff-action-base.cpp
@@ -148,29 +148,31 @@ void exposeDifferentialActionAbstract() {
           "g_lb",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_g_lb,
                             bp::return_internal_reference<>()),
+          &DifferentialActionModelAbstract_wrap::set_g_lb,
           "lower bound of the inequality constraints")
       .add_property(
           "g_ub",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_g_ub,
                             bp::return_internal_reference<>()),
+          &DifferentialActionModelAbstract_wrap::set_g_ub,
           "upper bound of the inequality constraints")
-      .add_property(
-          "has_control_limits",
-          bp::make_function(
-              &DifferentialActionModelAbstract_wrap::get_has_control_limits),
-          "indicates whether problem has finite control limits")
       .add_property(
           "u_lb",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_u_lb,
-                            bp::return_value_policy<bp::return_by_value>()),
+                            bp::return_internal_reference<>()),
           &DifferentialActionModelAbstract_wrap::set_u_lb,
           "lower control limits")
       .add_property(
           "u_ub",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_u_ub,
-                            bp::return_value_policy<bp::return_by_value>()),
+                            bp::return_internal_reference<>()),
           &DifferentialActionModelAbstract_wrap::set_u_ub,
           "upper control limits")
+      .add_property(
+          "has_control_limits",
+          bp::make_function(
+              &DifferentialActionModelAbstract_wrap::get_has_control_limits),
+          "indicates whether problem has finite control limits")
       .def(PrintableVisitor<DifferentialActionModelAbstract>());
 
   bp::register_ptr_to_python<

diff --git a/include/crocoddyl/core/action-base.hpp b/include/crocoddyl/core/action-base.hpp
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -245,12 +245,12 @@ class ActionModelAbstractTpl {
   /**
    * @brief Return the lower bound of the inequality constraints
    */
-  virtual const VectorXs& get_g_lb() const;
+  const VectorXs& get_g_lb() const;
 
   /**
    * @brief Return the upper bound of the inequality constraints
    */
-  virtual const VectorXs& get_g_ub() const;
+  const VectorXs& get_g_ub() const;
 
   /**
    * @brief Return the control lower bound
@@ -267,6 +267,16 @@ class ActionModelAbstractTpl {
    */
   bool get_has_control_limits() const;
 
+  /**
+   * @brief Modify the lower bound of the inequality constraints
+   */
+  void set_g_lb(const VectorXs& g_lb);
+
+  /**
+   * @brief Modify the upper bound of the inequality constraints
+   */
+  void set_g_ub(const VectorXs& g_ub);
+
   /**
    * @brief Modify the control lower bounds
    */

diff --git a/include/crocoddyl/core/action-base.hxx b/include/crocoddyl/core/action-base.hxx
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -170,11 +170,33 @@ bool ActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
   return has_control_limits_;
 }
 
+template <typename Scalar>
+void ActionModelAbstractTpl<Scalar>::set_g_lb(const VectorXs& g_lb) {
+  if (static_cast<std::size_t>(g_lb.size()) != ng_) {
+    throw_pretty(
+        "Invalid argument: "
+        << "inequality lower bound has wrong dimension (it should be " +
+               std::to_string(ng_) + ")");
+  }
+  g_lb_ = g_lb;
+}
+
+template <typename Scalar>
+void ActionModelAbstractTpl<Scalar>::set_g_ub(const VectorXs& g_ub) {
+  if (static_cast<std::size_t>(g_ub.size()) != ng_) {
+    throw_pretty(
+        "Invalid argument: "
+        << "inequality upper bound has wrong dimension (it should be " +
+               std::to_string(ng_) + ")");
+  }
+  g_ub_ = g_ub;
+}
+
 template <typename Scalar>
 void ActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
   if (static_cast<std::size_t>(u_lb.size()) != nu_) {
     throw_pretty("Invalid argument: "
-                 << "lower bound has wrong dimension (it should be " +
+                 << "control lower bound has wrong dimension (it should be " +
                         std::to_string(nu_) + ")");
   }
   u_lb_ = u_lb;
@@ -185,7 +207,7 @@ template <typename Scalar>
 void ActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
   if (static_cast<std::size_t>(u_ub.size()) != nu_) {
     throw_pretty("Invalid argument: "
-                 << "upper bound has wrong dimension (it should be " +
+                 << "control upper bound has wrong dimension (it should be " +
                         std::to_string(nu_) + ")");
   }
   u_ub_ = u_ub;

diff --git a/include/crocoddyl/core/diff-action-base.hpp b/include/crocoddyl/core/diff-action-base.hpp
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -278,12 +278,12 @@ class DifferentialActionModelAbstractTpl {
   /**
    * @brief Return the lower bound of the inequality constraints
    */
-  virtual const VectorXs& get_g_lb() const;
+  const VectorXs& get_g_lb() const;
 
   /**
    * @brief Return the upper bound of the inequality constraints
    */
-  virtual const VectorXs& get_g_ub() const;
+  const VectorXs& get_g_ub() const;
 
   /**
    * @brief Return the control lower bound
@@ -300,6 +300,16 @@ class DifferentialActionModelAbstractTpl {
    */
   bool get_has_control_limits() const;
 
+  /**
+   * @brief Modify the lower bound of the inequality constraints
+   */
+  void set_g_lb(const VectorXs& g_lb);
+
+  /**
+   * @brief Modify the upper bound of the inequality constraints
+   */
+  void set_g_ub(const VectorXs& g_ub);
+
   /**
    * @brief Modify the control lower bounds
    */

diff --git a/include/crocoddyl/core/diff-action-base.hxx b/include/crocoddyl/core/diff-action-base.hxx
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -68,8 +68,6 @@ void DifferentialActionModelAbstractTpl<Scalar>::quasiStatic(
   assert_pretty(x.tail(state_->get_nv()).isZero(),
                 "The velocity input should be zero for quasi-static to work.");
 
-  const std::size_t ndx = state_->get_ndx();
-  VectorXs dx = VectorXs::Zero(ndx);
   if (nu_ != 0) {
     VectorXs du = VectorXs::Zero(nu_);
     for (std::size_t i = 0; i < maxiter; ++i) {
@@ -169,6 +167,30 @@ bool DifferentialActionModelAbstractTpl<Scalar>::get_has_control_limits()
   return has_control_limits_;
 }
 
+template <typename Scalar>
+void DifferentialActionModelAbstractTpl<Scalar>::set_g_lb(
+    const VectorXs& g_lb) {
+  if (static_cast<std::size_t>(g_lb.size()) != ng_) {
+    throw_pretty(
+        "Invalid argument: "
+        << "inequality lower bound has wrong dimension (it should be " +
+               std::to_string(ng_) + ")");
+  }
+  g_lb_ = g_lb;
+}
+
+template <typename Scalar>
+void DifferentialActionModelAbstractTpl<Scalar>::set_g_ub(
+    const VectorXs& g_ub) {
+  if (static_cast<std::size_t>(g_ub.size()) != ng_) {
+    throw_pretty(
+        "Invalid argument: "
+        << "inequality upper bound has wrong dimension (it should be " +
+               std::to_string(ng_) + ")");
+  }
+  g_ub_ = g_ub;
+}
+
 template <typename Scalar>
 void DifferentialActionModelAbstractTpl<Scalar>::set_u_lb(
     const VectorXs& u_lb) {