Merge branch 'topic/more-finite-difference-helpers' into devel

CHANGELOG.md

@@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Changed
 
 - All map types are now `boost::unordered_map` ([#203](https://github.com/Simple-Robotics/aligator/pull/203))
+- Separate CostFiniteDifference out of finite-difference.hpp ([#212](https://github.com/Simple-Robotics/aligator/pull/212))
 
 ### Fixed
 

bindings/python/src/modelling/expose-autodiff.cpp

@@ -1,6 +1,7 @@
 #include "aligator/python/fwd.hpp"
 
 #include "aligator/modelling/autodiff/finite-difference.hpp"
+#include "aligator/modelling/autodiff/cost-finite-difference.hpp"
 
 namespace aligator {
 namespace python {

include/aligator/modelling/autodiff/cost-finite-difference.hpp

@@ -0,0 +1,58 @@
+#pragma once
+#include "aligator/core/cost-abstract.hpp"
+#include <proxsuite-nlp/manifold-base.hpp>
+
+namespace aligator {
+namespace autodiff {
+
+template <typename Scalar>
+struct CostFiniteDifferenceHelper : CostAbstractTpl<Scalar> {
+  using Manifold = ManifoldAbstractTpl<Scalar>;
+  using CostBase = CostAbstractTpl<Scalar>;
+  using CostData = CostDataAbstractTpl<Scalar>;
+
+  using CostBase::space;
+
+  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
+
+  struct Data;
+
+  CostFiniteDifferenceHelper(xyz::polymorphic<CostBase> cost,
+                             const Scalar fd_eps);
+
+  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
+                CostData &data_) const override;
+
+  void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u,
+                        CostData &data_) const override;
+
+  /// @brief Compute the cost Hessians \f$(\ell_{ij})_{i,j \in \{x,u\}}\f$
+  void computeHessians(const ConstVectorRef &, const ConstVectorRef &,
+                       CostData &) const override {}
+
+  auto createData() const -> shared_ptr<CostData> override;
+
+  xyz::polymorphic<CostBase> cost_;
+  Scalar fd_eps;
+};
+
+template <typename Scalar>
+struct CostFiniteDifferenceHelper<Scalar>::Data : CostData {
+
+  shared_ptr<CostData> c1, c2;
+  VectorXs dx, du;
+  VectorXs xp, up;
+
+  Data(CostFiniteDifferenceHelper const &obj)
+      : CostData(obj), dx(obj.ndx()), du(obj.nu), xp(obj.nx()), up(obj.nu) {
+    c1 = obj.cost_->createData();
+    c2 = obj.cost_->createData();
+  }
+};
+
+#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
+extern template struct CostFiniteDifferenceHelper<context::Scalar>;
+#endif
+
+} // namespace autodiff
+} // namespace aligator

include/aligator/modelling/autodiff/cost-finite-difference.hxx

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "aligator/modelling/autodiff/cost-finite-difference.hpp"
+
+namespace aligator::autodiff {
+
+template <typename Scalar>
+CostFiniteDifferenceHelper<Scalar>::CostFiniteDifferenceHelper(
+    xyz::polymorphic<CostBase> cost, const Scalar fd_eps)
+    : CostBase(cost->space, cost->nu), cost_(cost), fd_eps(fd_eps) {}
+
+template <typename Scalar>
+void CostFiniteDifferenceHelper<Scalar>::evaluate(const ConstVectorRef &x,
+                                                  const ConstVectorRef &u,
+                                                  CostData &data_) const {
+  Data &d = static_cast<Data &>(data_);
+  cost_->evaluate(x, u, *d.c1);
+
+  d.value_ = d.c1->value_;
+}
+
+template <typename Scalar>
+void CostFiniteDifferenceHelper<Scalar>::computeGradients(
+    const ConstVectorRef &x, const ConstVectorRef &u, CostData &data_) const {
+  Data &d = static_cast<Data &>(data_);
+  Manifold const &space = *this->space;
+
+  cost_->evaluate(x, u, *d.c1);
+
+  d.dx.setZero();
+  for (int i = 0; i < this->ndx(); i++) {
+    d.dx[i] = fd_eps;
+    space.integrate(x, d.dx, d.xp);
+    cost_->evaluate(d.xp, u, *d.c2);
+
+    d.Lx_[i] = (d.c2->value_ - d.c1->value_) / fd_eps;
+    d.dx[i] = 0.;
+  }
+
+  d.du.setZero();
+  for (int i = 0; i < this->nu; i++) {
+    d.du[i] = fd_eps;
+    d.up = u + d.du;
+    cost_->evaluate(x, d.up, *d.c2);
+
+    d.Lu_[i] = (d.c2->value_ - d.c1->value_) / fd_eps;
+    d.du[i] = 0.;
+  }
+}
+
+template <typename Scalar>
+auto CostFiniteDifferenceHelper<Scalar>::createData() const
+    -> shared_ptr<CostData> {
+  return std::make_shared<Data>(*this);
+}
+
+} // namespace aligator::autodiff

include/aligator/modelling/autodiff/finite-difference.hpp

@@ -3,7 +3,6 @@
 #pragma once
 
 #include "aligator/core/dynamics.hpp"
-#include "aligator/core/cost-abstract.hpp"
 #include <proxsuite-nlp/manifold-base.hpp>
 #include <boost/mpl/bool.hpp>
 
@@ -160,86 +159,9 @@ struct DynamicsFiniteDifferenceHelper
       : DynamicsModel(space, func->nu, space), Base(space, func, fd_eps) {}
 };
 
-template <typename Scalar>
-struct CostFiniteDifferenceHelper : CostAbstractTpl<Scalar> {
-  using Manifold = ManifoldAbstractTpl<Scalar>;
-  using CostBase = CostAbstractTpl<Scalar>;
-  using CostData = CostDataAbstractTpl<Scalar>;
-
-  using CostBase::space;
-
-  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
-
-  struct Data : CostData {
-
-    shared_ptr<CostData> c1, c2;
-    VectorXs dx, du;
-    VectorXs xp, up;
-
-    Data(CostFiniteDifferenceHelper const &obj)
-        : CostData(obj), dx(obj.ndx()), du(obj.nu), xp(obj.nx()), up(obj.nu) {
-      c1 = obj.cost_->createData();
-      c2 = obj.cost_->createData();
-    }
-  };
-
-  CostFiniteDifferenceHelper(xyz::polymorphic<CostBase> cost,
-                             const Scalar fd_eps)
-
-      : CostBase(cost->space, cost->nu), cost_(cost), fd_eps(fd_eps) {}
-
-  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
-                CostData &data_) const override {
-    Data &d = static_cast<Data &>(data_);
-    cost_->evaluate(x, u, *d.c1);
-
-    d.value_ = d.c1->value_;
-  }
-
-  void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u,
-                        CostData &data_) const override {
-    Data &d = static_cast<Data &>(data_);
-    Manifold const &space = *this->space;
-
-    cost_->evaluate(x, u, *d.c1);
-
-    d.dx.setZero();
-    for (int i = 0; i < this->ndx(); i++) {
-      d.dx[i] = fd_eps;
-      space.integrate(x, d.dx, d.xp);
-      cost_->evaluate(d.xp, u, *d.c2);
-
-      d.Lx_[i] = (d.c2->value_ - d.c1->value_) / fd_eps;
-      d.dx[i] = 0.;
-    }
-
-    d.du.setZero();
-    for (int i = 0; i < this->nu; i++) {
-      d.du[i] = fd_eps;
-      d.up = u + d.du;
-      cost_->evaluate(x, d.up, *d.c2);
-
-      d.Lu_[i] = (d.c2->value_ - d.c1->value_) / fd_eps;
-      d.du[i] = 0.;
-    }
-  }
-
-  /// @brief Compute the cost Hessians \f$(\ell_{ij})_{i,j \in \{x,u\}}\f$
-  void computeHessians(const ConstVectorRef &, const ConstVectorRef &,
-                       CostData &) const override {}
-
-  shared_ptr<CostData> createData() const override {
-    return std::make_shared<Data>(*this);
-  }
-
-  xyz::polymorphic<CostBase> cost_;
-  Scalar fd_eps;
-};
-
 #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
 extern template struct FiniteDifferenceHelper<context::Scalar>;
 extern template struct DynamicsFiniteDifferenceHelper<context::Scalar>;
-extern template struct CostFiniteDifferenceHelper<context::Scalar>;
 #endif
 
 } // namespace autodiff

src/modelling/autodiff/cost-finite-difference.cpp

@@ -0,0 +1,7 @@
+#include "aligator/modelling/autodiff/cost-finite-difference.hxx"
+
+namespace aligator::autodiff {
+
+template struct CostFiniteDifferenceHelper<context::Scalar>;
+
+} // namespace aligator::autodiff

src/modelling/autodiff/finite-difference.cpp

@@ -5,7 +5,6 @@ namespace autodiff {
 
 template struct FiniteDifferenceHelper<context::Scalar>;
 template struct DynamicsFiniteDifferenceHelper<context::Scalar>;
-template struct CostFiniteDifferenceHelper<context::Scalar>;
 
 } // namespace autodiff
 } // namespace aligator