Merge pull request #1176 from cmastalli/topic/control-residual

loco-3d · Nov 4, 2023 · 4c629de · 4c629de
2 parents 06feac6 + 01d3c4b
commit 4c629de
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 2 deletions.
diff --git a/bindings/python/crocoddyl/multibody/friction-cone.cpp b/bindings/python/crocoddyl/multibody/friction-cone.cpp
@@ -34,7 +34,7 @@ void exposeFrictionCone() {
           ":param R: rotation matrix that defines the cone orientation w.r.t. "
           "the inertial frame\n"
           ":param mu: friction coefficient\n"
-          ":param nf: number of facets\n"
+          ":param nf: number of facets (default 4)\n"
           ":param inner_appr: inner or outer approximation (default True)\n"
           ":param min_nforce: minimum normal force (default 0.)\n"
           ":param max_nforce: maximum normal force (default "

diff --git a/include/crocoddyl/core/residuals/control.hpp b/include/crocoddyl/core/residuals/control.hpp
@@ -1,7 +1,8 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
+//                          Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -112,6 +113,24 @@ class ResidualModelControlTpl : public ResidualModelAbstractTpl<_Scalar> {
   virtual boost::shared_ptr<ResidualDataAbstract> createData(
       DataCollectorAbstract* const data);
 
+  /**
+   * @brief Compute the derivative of the control-cost function
+   *
+   * This function assumes that the derivatives of the activation and residual
+   * are computed via calcDiff functions.
+   *
+   * @param cdata     Cost data
+   * @param rdata     Residual data
+   * @param adata     Activation data
+   * @param update_u  Update the derivative of the cost function w.r.t. to the
+   * control if True.
+   */
+  virtual void calcCostDiff(
+      const boost::shared_ptr<CostDataAbstract>& cdata,
+      const boost::shared_ptr<ResidualDataAbstract>& rdata,
+      const boost::shared_ptr<ActivationDataAbstract>& adata,
+      const bool update_u = true);
+
   /**
    * @brief Return the reference control vector
    */

diff --git a/include/crocoddyl/core/residuals/control.hxx b/include/crocoddyl/core/residuals/control.hxx
@@ -91,6 +91,15 @@ ResidualModelControlTpl<Scalar>::createData(
   return data;
 }
 
+template <typename Scalar>
+void ResidualModelControlTpl<Scalar>::calcCostDiff(
+    const boost::shared_ptr<CostDataAbstract>& cdata,
+    const boost::shared_ptr<ResidualDataAbstract>&,
+    const boost::shared_ptr<ActivationDataAbstract>& adata, const bool) {
+  cdata->Lu = adata->Ar;
+  cdata->Luu = adata->Arr;
+}
+
 template <typename Scalar>
 void ResidualModelControlTpl<Scalar>::print(std::ostream& os) const {
   os << "ResidualModelControl";