Merge pull request #1269 from cmastalli/topic/terminal-dimension

CHANGELOG.md

@@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+* Introduced the notion of terminal dimension, residuals and constraints in https://github.com/loco-3d/crocoddyl/pull/1269
 * General clean ups about std::size_t in https://github.com/loco-3d/crocoddyl/pull/1265
 * Fixed issues in LQR extensions in https://github.com/loco-3d/crocoddyl/pull/1263
 * Computed dynamic feasibility everytime in https://github.com/loco-3d/crocoddyl/pull/1262

bindings/python/crocoddyl/__init__.py

@@ -6,6 +6,7 @@
 
 import numpy as np
 import pinocchio
+from pinocchio.visualize import MeshcatVisualizer
 
 from .libcrocoddyl_pywrap import *  # noqa: F403
 from .libcrocoddyl_pywrap import __raw_version__, __version__  # noqa: F401
@@ -558,7 +559,7 @@ def __init__(
         if frameNames is not None:
             print("Deprecated. Do not pass frameNames")
         robot.setVisualizer(
-            pinocchio.visualize.MeshcatVisualizer(
+            MeshcatVisualizer(
                 model=self.robot.model,
                 collision_model=self.robot.collision_model,
                 visual_model=self.robot.visual_model,

bindings/python/crocoddyl/core/action-base.cpp

@@ -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,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -40,15 +40,18 @@ void exposeActionAbstract() {
       "and calcDiff(),\n"
       "respectively.",
       bp::init<boost::shared_ptr<StateAbstract>, std::size_t,
-               bp::optional<std::size_t, std::size_t, std::size_t> >(
-          bp::args("self", "state", "nu", "nr", "ng", "nh"),
+               bp::optional<std::size_t, std::size_t, std::size_t, std::size_t,
+                            std::size_t> >(
+          bp::args("self", "state", "nu", "nr", "ng", "nh", "ng_T", "nh_T"),
           "Initialize the action model.\n\n"
           "We can also describe autonomous systems by setting nu = 0.\n"
           ":param state: state description,\n"
           ":param nu: dimension of control vector,\n"
           ":param nr: dimension of the cost-residual vector (default 1)\n"
           ":param ng: number of inequality constraints (default 0)\n"
-          ":param nh: number of equality constraints (default 0)\n"))
+          ":param nh: number of equality constraints (default 0)\n"
+          ":param ng_T: number of inequality terminal constraints (default 0)\n"
+          ":param nh_T: number of equality terminal constraints (default 0)"))
       .def("calc", pure_virtual(&ActionModelAbstract_wrap::calc),
            bp::args("self", "data", "x", "u"),
            "Compute the next state and cost value.\n\n"
@@ -130,6 +133,16 @@ void exposeActionAbstract() {
                     bp::make_setter(&ActionModelAbstract_wrap::nh_,
                                     bp::return_internal_reference<>()),
                     "number of equality constraints")
+      .add_property("ng_T",
+                    bp::make_function(&ActionModelAbstract_wrap::get_ng_T),
+                    bp::make_setter(&ActionModelAbstract_wrap::ng_T_,
+                                    bp::return_internal_reference<>()),
+                    "number of inequality terminal constraints")
+      .add_property("nh_T",
+                    bp::make_function(&ActionModelAbstract_wrap::get_nh_T),
+                    bp::make_setter(&ActionModelAbstract_wrap::nh_T_,
+                                    bp::return_internal_reference<>()),
+                    "number of equality terminal constraints")
       .add_property(
           "state",
           bp::make_function(&ActionModelAbstract_wrap::get_state,

bindings/python/crocoddyl/core/action-base.hpp

@@ -22,14 +22,18 @@ class ActionModelAbstract_wrap : public ActionModelAbstract,
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   using ActionModelAbstract::ng_;
+  using ActionModelAbstract::ng_T_;
   using ActionModelAbstract::nh_;
+  using ActionModelAbstract::nh_T_;
   using ActionModelAbstract::nu_;
   using ActionModelAbstract::unone_;
 
   ActionModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
                            const std::size_t nu, const std::size_t nr = 1,
-                           const std::size_t ng = 0, const std::size_t nh = 0)
-      : ActionModelAbstract(state, nu, nr, ng, nh),
+                           const std::size_t ng = 0, const std::size_t nh = 0,
+                           const std::size_t ng_T = 0,
+                           const std::size_t nh_T = 0)
+      : ActionModelAbstract(state, nu, nr, ng, nh, ng_T, nh_T),
         bp::wrapper<ActionModelAbstract>() {
     unone_ = NAN * MathBase::VectorXs::Ones(nu);
   }

bindings/python/crocoddyl/core/constraint-base.cpp

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2020-2023, University of Edinburgh, Heriot-Watt University
+// Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -42,19 +42,28 @@ void exposeConstraintAbstract() {
                             ":param ng: number of inequality constraints\n"
                             ":param nh: number of equality constraints"))
       .def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t, std::size_t,
-                    std::size_t>(
-          bp::args("self", "state", "nu", "ng", "nh"),
+                    std::size_t, bp::optional<bool> >(
+          bp::args("self", "state", "nu", "ng", "nh", "T_const"),
           "Initialize the constraint model.\n\n"
           ":param state: state description\n"
           ":param nu: dimension of control vector (default state.nv)\n"
           ":param ng: number of inequality constraints\n"
-          ":param nh: number of equality constraints"))
-      .def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t, std::size_t>(
-          bp::args("self", "state", "ng", "nh"),
+          ":param nh: number of equality constraints\n"
+          ":param T_const: True if this is a constraint in both running and "
+          "terminal nodes.\n"
+          "                False if it is a constraint on running nodes only "
+          "(default true)"))
+      .def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t, std::size_t,
+                    bp::optional<bool> >(
+          bp::args("self", "state", "ng", "nh", "T_const"),
           "Initialize the constraint model.\n\n"
           ":param state: state description\n"
           ":param ng: number of inequality constraints\n"
-          ":param nh: number of equality constraints"))
+          ":param nh: number of equality constraints\n"
+          ":param T_const: True if this is a constraint in both running and "
+          "terminal nodes.\n"
+          "                False if it is a constraint on running nodes only "
+          "(default true)"))
       .def("calc", pure_virtual(&ConstraintModelAbstract_wrap::calc),
            bp::args("self", "data", "x", "u"),
            "Compute the constraint value.\n\n"
@@ -141,6 +150,10 @@ void exposeConstraintAbstract() {
       .add_property("nh",
                     bp::make_function(&ConstraintModelAbstract_wrap::get_nh),
                     "number of equality constraints")
+      .add_property(
+          "T_constraint",
+          bp::make_function(&ConstraintModelAbstract_wrap::get_T_constraint),
+          "True if the constraint is imposed in terminal nodes as well")
       .def(PrintableVisitor<ConstraintModelAbstract>());
 
   bp::register_ptr_to_python<boost::shared_ptr<ConstraintDataAbstract> >();

bindings/python/crocoddyl/core/constraint-base.hpp

@@ -35,15 +35,16 @@ class ConstraintModelAbstract_wrap
 
   ConstraintModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
                                const std::size_t nu, const std::size_t ng,
-                               const std::size_t nh)
-      : ConstraintModelAbstract(state, nu, ng, nh),
+                               const std::size_t nh, const bool T_const = true)
+      : ConstraintModelAbstract(state, nu, ng, nh, T_const),
         bp::wrapper<ConstraintModelAbstract>() {
     unone_ = NAN * MathBase::VectorXs::Ones(nu);
   }
 
   ConstraintModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
-                               const std::size_t ng, const std::size_t nh)
-      : ConstraintModelAbstract(state, ng, nh) {
+                               const std::size_t ng, const std::size_t nh,
+                               const bool T_const = true)
+      : ConstraintModelAbstract(state, ng, nh, T_const) {
     unone_ = NAN * MathBase::VectorXs::Ones(nu_);
   }
 

bindings/python/crocoddyl/core/constraints/constraint-manager.cpp

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2020-2023, University of Edinburgh, Heriot-Watt University
+// Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -28,6 +28,10 @@ namespace python {
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(
     ConstraintModelManager_addConstraint_wrap,
     ConstraintModelManager::addConstraint, 2, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ConstraintDataManager_resizeConst_wrap,
+                                       ConstraintDataManager::resize, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ConstraintDataManager_resize_wrap,
+                                       ConstraintDataManager::resize, 2, 3)
 
 void exposeConstraintManager() {
   // Register custom converters between std::map and Python dict
@@ -169,6 +173,12 @@ void exposeConstraintManager() {
                     "number of active inequality constraints")
       .add_property("nh", bp::make_function(&ConstraintModelManager::get_nh),
                     "number of active equality constraints")
+      .add_property("ng_T",
+                    bp::make_function(&ConstraintModelManager::get_ng_T),
+                    "number of active inequality terminal constraints")
+      .add_property("nh_T",
+                    bp::make_function(&ConstraintModelManager::get_nh_T),
+                    "number of active equality terminal constraints")
       .add_property(
           "active_set",
           bp::make_function(&ConstraintModelManager::get_active_set,
@@ -214,23 +224,41 @@ void exposeConstraintManager() {
            bp::args("self", "data"),
            "Share memory with a given action data\n\n"
            ":param model: action data that we want to share memory")
+      .def("resize", &ConstraintDataManager::resize<ConstraintModelManager>,
+           bp::with_custodian_and_ward_postcall<0, 1>(),
+           ConstraintDataManager_resizeConst_wrap(
+               bp::args("self", "model", "running_node"),
+               "Resize the data given differential action data\n\n"
+               ":param model: constraint manager model that defines how to "
+               "resize "
+               "the data\n"
+               ":param running_node: true if we are resizing for a running "
+               "node, false for terminal ones (default true)"))
       .def("resize",
            &ConstraintDataManager::resize<DifferentialActionModelAbstract,
                                           DifferentialActionDataAbstract>,
            bp::with_custodian_and_ward_postcall<0, 2>(),
-           bp::args("self", "model", "data"),
-           "Resize the data given differential action data\n\n"
-           ":param model: differential action model that defines how to resize "
-           "the data\n"
-           ":param data: differential action data that we want to resize")
-      .def("resize",
-           &ConstraintDataManager::resize<ActionModelAbstract,
-                                          ActionDataAbstract>,
-           bp::with_custodian_and_ward_postcall<0, 2>(),
-           bp::args("self", "model", "data"),
-           "Resize the data given action data\n\n"
-           ":param model: action model that defines how to resize the data\n"
-           ":param data: action data that we want to resize")
+           ConstraintDataManager_resize_wrap(
+               bp::args("self", "model", "data", "running_node"),
+               "Resize the data given differential action data\n\n"
+               ":param model: differential action model that defines how to "
+               "resize "
+               "the data\n"
+               ":param data: differential action data that we want to resize\n"
+               ":param running_node: true if we are resizing for a running "
+               "node, false for terminal ones (default true)"))
+      .def(
+          "resize",
+          &ConstraintDataManager::resize<ActionModelAbstract,
+                                         ActionDataAbstract>,
+          bp::with_custodian_and_ward_postcall<0, 2>(),
+          ConstraintDataManager_resize_wrap(
+              bp::args("self", "model", "data", "running_node"),
+              "Resize the data given action data\n\n"
+              ":param model: action model that defines how to resize the data\n"
+              ":param data: action data that we want to resize\n"
+              ":param running_node: true if we are resizing for a running "
+              "node, false for terminal ones (default true)"))
       .add_property(
           "constraints",
           bp::make_getter(&ConstraintDataManager::constraints,

bindings/python/crocoddyl/core/constraints/residual.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.
 ///////////////////////////////////////////////////////////////////////////////
@@ -23,21 +23,26 @@ void exposeConstraintResidual() {
       "vector and its bounds.",
       bp::init<boost::shared_ptr<StateAbstract>,
                boost::shared_ptr<ResidualModelAbstract>, Eigen::VectorXd,
-               Eigen::VectorXd>(
-          bp::args("self", "state", "residual", "lower", "upper"),
+               Eigen::VectorXd, bp::optional<bool> >(
+          bp::args("self", "state", "residual", "lower", "upper", "T_act"),
           "Initialize the residual constraint model as an inequality "
           "constraint.\n\n"
           ":param state: state description\n"
           ":param residual: residual model\n"
           ":param lower: lower bound\n"
-          ":param upper: upper bound"))
+          ":param upper: upper bound\n"
+          ":param T_act: false if we want to deactivate the residual at the "
+          "terminal node (default true)"))
       .def(bp::init<boost::shared_ptr<StateAbstract>,
-                    boost::shared_ptr<ResidualModelAbstract> >(
-          bp::args("self", "state", "residual"),
+                    boost::shared_ptr<ResidualModelAbstract>,
+                    bp::optional<bool> >(
+          bp::args("self", "state", "residual", "T_act"),
           "Initialize the residual constraint model as an equality "
           "constraint.\n\n"
           ":param state: state description\n"
-          ":param residual: residual model"))
+          ":param residual: residual model\n"
+          ":param T_act: false if we want to deactivate the residual at the "
+          "terminal node (default true)"))
       .def<void (ConstraintModelResidual::*)(
           const boost::shared_ptr<ConstraintDataAbstract>&,
           const Eigen::Ref<const Eigen::VectorXd>&,

bindings/python/crocoddyl/core/diff-action-base.cpp

@@ -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,
 //                          University of Oxford, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -44,15 +44,18 @@ void exposeDifferentialActionAbstract() {
       "computations are\n"
       "mainly carried out inside calc() and calcDiff(), respectively.",
       bp::init<boost::shared_ptr<StateAbstract>, std::size_t,
-               bp::optional<std::size_t, std::size_t, std::size_t> >(
-          bp::args("self", "state", "nu", "nr", "ng", "nh"),
+               bp::optional<std::size_t, std::size_t, std::size_t, std::size_t,
+                            std::size_t> >(
+          bp::args("self", "state", "nu", "nr", "ng", "nh", "ng_T", "nh_T"),
           "Initialize the differential action model.\n\n"
           "We can also describe autonomous systems by setting nu = 0.\n"
           ":param state: state\n"
           ":param nu: dimension of control vector\n"
           ":param nr: dimension of cost-residual vector (default 1)\n"
           ":param ng: number of inequality constraints (default 0)\n"
-          ":param nh: number of equality constraints (default 0)"))
+          ":param nh: number of equality constraints (default 0)\n"
+          ":param ng_T: number of inequality terminal constraints (default 0)\n"
+          ":param nh_T: number of equality terminal constraints (default 0)"))
       .def("calc", pure_virtual(&DifferentialActionModelAbstract_wrap::calc),
            bp::args("self", "data", "x", "u"),
            "Compute the system acceleration and cost value.\n\n"
@@ -139,6 +142,14 @@ void exposeDifferentialActionAbstract() {
           "nh",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_nh),
           "number of equality constraints")
+      .add_property(
+          "ng_T",
+          bp::make_function(&DifferentialActionModelAbstract_wrap::get_ng_T),
+          "number of inequality terminal constraints")
+      .add_property(
+          "nh_T",
+          bp::make_function(&DifferentialActionModelAbstract_wrap::get_nh_T),
+          "number of equality terminal constraints")
       .add_property(
           "state",
           bp::make_function(&DifferentialActionModelAbstract_wrap::get_state,

bindings/python/crocoddyl/core/diff-action-base.hpp

@@ -28,8 +28,10 @@ class DifferentialActionModelAbstract_wrap
                                        const std::size_t nu,
                                        const std::size_t nr = 1,
                                        const std::size_t ng = 0,
-                                       const std::size_t nh = 0)
-      : DifferentialActionModelAbstract(state, nu, nr, ng, nh),
+                                       const std::size_t nh = 0,
+                                       const std::size_t ng_T = 0,
+                                       const std::size_t nh_T = 0)
+      : DifferentialActionModelAbstract(state, nu, nr, ng, nh, ng_T, nh_T),
         bp::wrapper<DifferentialActionModelAbstract>() {
     unone_ = NAN * MathBase::VectorXs::Ones(nu);
   }