7#include <proxsuite-nlp/manifold-base.hpp>
8#include <boost/mpl/bool.hpp>
15template <
typename _Scalar,
template <
typename>
class _BaseTpl>
16struct finite_diff_traits;
18template <
typename Scalar>
struct finite_diff_traits<
Scalar, StageFunctionTpl> {
20 struct Data : StageFunctionDataTpl<Scalar> {
21 using SFD = StageFunctionDataTpl<Scalar>;
24 shared_ptr<SFD> data_0;
25 shared_ptr<SFD> data_1;
31 : SFD(*model.func_), data_0(model.func_->createData()),
32 data_1(model.func_->createData()), dx(ndx1), du(nu), xp(model.nx1),
45template <
typename Scalar>
struct finite_diff_traits<
Scalar, DynamicsModelTpl> {
47 struct Data : DynamicsDataTpl<Scalar> {
48 using DD = DynamicsDataTpl<Scalar>;
52 shared_ptr<DD> data_0;
53 shared_ptr<DD> data_1;
59 : DD(*model.func_), data_0(model.func_->createData()),
60 data_1(model.func_->createData()), dx(ndx1), du(nu), dy(ndx2),
61 xp(model.nx1), up(model.nu), yp(model.nx2) {
76template <
typename _Scalar,
template <
typename>
class _BaseTpl>
77struct finite_difference_impl : finite_diff_traits<_Scalar, _BaseTpl> {
78 using Scalar = _Scalar;
80 using Traits = finite_diff_traits<Scalar, _BaseTpl>;
81 using Data =
typename Traits::Data;
82 using Args =
typename Traits::Args;
83 using Base = _BaseTpl<Scalar>;
84 using BaseData =
typename Base::Data;
85 using Manifold = ManifoldAbstractTpl<Scalar>;
87 static_assert(std::is_base_of_v<BaseData, Data>);
89 xyz::polymorphic<Manifold> space_;
90 xyz::polymorphic<Base> func_;
94 static constexpr bool IsDynamics =
95 std::is_same_v<Base, DynamicsModelTpl<Scalar>>;
97 template <
typename U = Base,
class = std::enable_if_t<
98 std::is_same_v<U, StageFunctionTpl<Scalar>>>>
99 finite_difference_impl(xyz::polymorphic<Manifold> space,
100 xyz::polymorphic<U> func,
const Scalar fd_eps)
101 : space_(space), func_(func), fd_eps(fd_eps), nx1(space->nx()),
104 template <
typename U = Base,
class = std::enable_if_t<
105 std::is_same_v<U, DynamicsModelTpl<Scalar>>>>
106 finite_difference_impl(xyz::polymorphic<Manifold> space,
107 xyz::polymorphic<U> func,
const Scalar fd_eps,
108 boost::mpl::false_ = {})
109 : space_(space), func_(func), fd_eps(fd_eps), nx1(space->nx()),
110 nu(func->nu), nx2(space->nx()) {}
114 void evaluateImpl(
const Args &args, BaseData &data)
const {
115 Data &d =
static_cast<Data &
>(
data);
118 if constexpr (IsDynamics) {
119 func_->evaluate(args.x, args.u, args.y, *d.data_0);
121 func_->evaluate(args.x, args.u, *d.data_0);
123 d.value_ = d.data_0->value_;
126 void computeJacobiansImpl(
const Args &args, BaseData &data)
const {
127 Data &d =
static_cast<Data &
>(
data);
131 VectorXs &v0 = d.data_0->value_;
132 VectorXs &vp = d.data_1->value_;
134 for (
int i = 0; i < func_->ndx1; i++) {
136 space_->integrate(args.x, d.dx, d.xp);
137 if constexpr (IsDynamics) {
138 func_->evaluate(d.xp, args.u, args.y, *d.data_1);
140 func_->evaluate(d.xp, args.u, *d.data_1);
142 data.Jx_.col(i) = (vp - v0) / fd_eps;
146 if constexpr (IsDynamics) {
147 for (
int i = 0; i < func_->ndx2; i++) {
149 space_->integrate(args.y, d.dy, d.yp);
150 func_->evaluate(args.x, args.u, d.yp, *d.data_1);
151 data.Jy_.col(i) = (vp - v0) / fd_eps;
156 for (
int i = 0; i < func_->nu; i++) {
158 d.up = args.u + d.du;
159 if constexpr (IsDynamics) {
160 func_->evaluate(args.x, d.up, args.y, *d.data_1);
162 func_->evaluate(args.x, d.up, *d.data_1);
164 data.Ju_.col(i) = (vp - v0) / fd_eps;
169 void computeVectorHessianProductsImpl(
const ConstVectorRef &,
170 const ConstVectorRef &,
171 const ConstVectorRef &,
172 const ConstVectorRef &,
175 shared_ptr<BaseData> createDataImpl()
const {
176 return std::make_shared<Data>(*
this);
188template <
typename _Scalar>
191 using Impl = internal::finite_difference_impl<Scalar, StageFunctionTpl>;
195 using Data =
typename Impl::Data;
201 xyz::polymorphic<StageFunction> func,
204 impl(space, func, fd_eps) {}
206 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
208 impl.evaluateImpl({x, u}, data);
213 impl.computeJacobiansImpl({x, u}, data);
216 shared_ptr<BaseData>
createData()
const {
return impl.createDataImpl(); }
222template <
typename _Scalar>
228 using Impl = internal::finite_difference_impl<Scalar, DynamicsModelTpl>;
229 using Data =
typename Impl::Data;
235 xyz::polymorphic<DynamicsModel> func,
239 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
240 const ConstVectorRef &xn,
BaseData &data)
const {
241 impl.evaluateImpl({x, u, xn}, data);
245 const ConstVectorRef &xn,
BaseData &data)
const {
246 impl.computeJacobiansImpl({x, u, xn}, data);
249 shared_ptr<BaseData>
createData()
const {
return impl.createDataImpl(); }
255#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
256extern template struct FiniteDifferenceHelper<context::Scalar>;
257extern template struct DynamicsFiniteDifferenceHelper<context::Scalar>;
Base definitions for ternary functions.
#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
::aligator::context::Scalar Scalar
constexpr auto data(C &c) noexcept(noexcept(c.data())) -> decltype(c.data())
const Manifold & space() const
State space for the input.
const int nu
Control dimension.
DynamicsModelTpl(xyz::polymorphic< Manifold > space, const int nu)
Constructor for dynamics.
Base struct for function data.
const int nu
Control dimension.
const int ndx1
Current state dimension.
const int nr
Function codimension.
StageFunctionTpl(const int ndx, const int nu, const int nr)
DynamicsDataTpl< Scalar > BaseData
internal::finite_difference_impl< Scalar, DynamicsModelTpl > Impl
DynamicsFiniteDifferenceHelper(xyz::polymorphic< Manifold > space, xyz::polymorphic< DynamicsModel > func, const Scalar fd_eps)
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, const ConstVectorRef &xn, BaseData &data) const
ManifoldAbstractTpl< Scalar > Manifold
DynamicsModelTpl< Scalar > DynamicsModel
ALIGATOR_DYNAMIC_TYPEDEFS(_Scalar)
shared_ptr< BaseData > createData() const
void computeJacobians(const ConstVectorRef &x, const ConstVectorRef &u, const ConstVectorRef &xn, BaseData &data) const
Approximate the derivatives of a given function using finite differences, to downcast the function to...
ManifoldAbstractTpl< Scalar > Manifold
internal::finite_difference_impl< Scalar, StageFunctionTpl > Impl
StageFunctionDataTpl< Scalar > BaseData
StageFunctionTpl< Scalar > StageFunction
ALIGATOR_DYNAMIC_TYPEDEFS(_Scalar)
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Evaluate the function.
void computeJacobians(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Compute Jacobians of this function.
FiniteDifferenceHelper(xyz::polymorphic< Manifold > space, xyz::polymorphic< StageFunction > func, const Scalar fd_eps)
shared_ptr< BaseData > createData() const
Instantiate a Data object.