aligator  0.12.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
 
Loading...
Searching...
No Matches
finite-difference.hpp
Go to the documentation of this file.
1
3#pragma once
4
7#include <proxsuite-nlp/manifold-base.hpp>
8#include <boost/mpl/bool.hpp>
9
10namespace aligator {
11namespace autodiff {
12
13namespace internal {
14
15template <typename _Scalar, template <typename> class _BaseTpl>
16struct finite_diff_traits;
17
18template <typename Scalar> struct finite_diff_traits<Scalar, StageFunctionTpl> {
20 struct Data : StageFunctionDataTpl<Scalar> {
21 using SFD = StageFunctionDataTpl<Scalar>;
22 using SFD::ndx1;
23 using SFD::nu;
24 shared_ptr<SFD> data_0;
25 shared_ptr<SFD> data_1;
26 VectorXs dx, du;
27 VectorXs xp, up;
28
29 template <typename U>
30 Data(U const &model)
31 : SFD(*model.func_), data_0(model.func_->createData()),
32 data_1(model.func_->createData()), dx(ndx1), du(nu), xp(model.nx1),
33 up(model.nu) {
34 dx.setZero();
35 du.setZero();
36 }
37 };
38
39 struct Args {
40 ConstVectorRef x;
41 ConstVectorRef u;
42 };
43};
44
45template <typename Scalar> struct finite_diff_traits<Scalar, DynamicsModelTpl> {
47 struct Data : DynamicsDataTpl<Scalar> {
48 using DD = DynamicsDataTpl<Scalar>;
49 using DD::ndx1;
50 using DD::ndx2;
51 using DD::nu;
52 shared_ptr<DD> data_0;
53 shared_ptr<DD> data_1;
54 VectorXs dx, du, dy;
55 VectorXs xp, up, yp;
56
57 template <typename U>
58 Data(U const &model)
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) {
62 dx.setZero();
63 du.setZero();
64 dy.setZero();
65 }
66 };
67
68 struct Args {
69 ConstVectorRef x;
70 ConstVectorRef u;
71 ConstVectorRef y;
72 };
73};
74
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>;
86
87 static_assert(std::is_base_of_v<BaseData, Data>);
88
89 xyz::polymorphic<Manifold> space_;
90 xyz::polymorphic<Base> func_;
91 Scalar fd_eps;
92 int nx1, nu, nx2;
93
94 static constexpr bool IsDynamics =
95 std::is_same_v<Base, DynamicsModelTpl<Scalar>>;
96
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()),
102 nx2(space->nx()) {}
103
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()) {}
111
114 void evaluateImpl(const Args &args, BaseData &data) const {
115 Data &d = static_cast<Data &>(data);
116 assert(d.data_0);
117 assert(d.data_1);
118 if constexpr (IsDynamics) {
119 func_->evaluate(args.x, args.u, args.y, *d.data_0);
120 } else {
121 func_->evaluate(args.x, args.u, *d.data_0);
122 }
123 d.value_ = d.data_0->value_;
124 }
125
126 void computeJacobiansImpl(const Args &args, BaseData &data) const {
127 Data &d = static_cast<Data &>(data);
128 assert(d.data_0);
129 assert(d.data_1);
130
131 VectorXs &v0 = d.data_0->value_;
132 VectorXs &vp = d.data_1->value_;
133
134 for (int i = 0; i < func_->ndx1; i++) {
135 d.dx[i] = fd_eps;
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);
139 } else {
140 func_->evaluate(d.xp, args.u, *d.data_1);
141 }
142 data.Jx_.col(i) = (vp - v0) / fd_eps;
143 d.dx[i] = 0.;
144 }
145
146 if constexpr (IsDynamics) {
147 for (int i = 0; i < func_->ndx2; i++) {
148 d.dy[i] = fd_eps;
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;
152 d.dy[i] = 0.;
153 }
154 }
155
156 for (int i = 0; i < func_->nu; i++) {
157 d.du[i] = fd_eps;
158 d.up = args.u + d.du;
159 if constexpr (IsDynamics) {
160 func_->evaluate(args.x, d.up, args.y, *d.data_1);
161 } else {
162 func_->evaluate(args.x, d.up, *d.data_1);
163 }
164 data.Ju_.col(i) = (vp - v0) / fd_eps;
165 d.du[i] = 0.;
166 }
167 }
168
169 void computeVectorHessianProductsImpl(const ConstVectorRef &,
170 const ConstVectorRef &,
171 const ConstVectorRef &,
172 const ConstVectorRef &,
173 BaseData &) const {}
174
175 shared_ptr<BaseData> createDataImpl() const {
176 return std::make_shared<Data>(*this);
177 }
178};
179
180} // namespace internal
181
182template <typename _Scalar> struct FiniteDifferenceHelper;
183
188template <typename _Scalar>
190 using Scalar = _Scalar;
191 using Impl = internal::finite_difference_impl<Scalar, StageFunctionTpl>;
192
194 using Manifold = ManifoldAbstractTpl<Scalar>;
195 using Data = typename Impl::Data;
197
199
200 FiniteDifferenceHelper(xyz::polymorphic<Manifold> space,
201 xyz::polymorphic<StageFunction> func,
202 const Scalar fd_eps)
203 : StageFunction(func->ndx1, func->nu, func->nr),
204 impl(space, func, fd_eps) {}
205
206 void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
207 BaseData &data) const {
208 impl.evaluateImpl({x, u}, data);
209 }
210
211 void computeJacobians(const ConstVectorRef &x, const ConstVectorRef &u,
212 BaseData &data) const {
213 impl.computeJacobiansImpl({x, u}, data);
214 }
215
216 shared_ptr<BaseData> createData() const { return impl.createDataImpl(); }
217
218private:
219 Impl impl;
220};
221
222template <typename _Scalar>
224 using Scalar = _Scalar;
225
227 using Manifold = ManifoldAbstractTpl<Scalar>;
228 using Impl = internal::finite_difference_impl<Scalar, DynamicsModelTpl>;
229 using Data = typename Impl::Data;
231
233
234 DynamicsFiniteDifferenceHelper(xyz::polymorphic<Manifold> space,
235 xyz::polymorphic<DynamicsModel> func,
236 const Scalar fd_eps)
237 : DynamicsModel(space, func->nu, space), impl(space, func, fd_eps) {}
238
239 void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
240 const ConstVectorRef &xn, BaseData &data) const {
241 impl.evaluateImpl({x, u, xn}, data);
242 }
243
244 void computeJacobians(const ConstVectorRef &x, const ConstVectorRef &u,
245 const ConstVectorRef &xn, BaseData &data) const {
246 impl.computeJacobiansImpl({x, u, xn}, data);
247 }
248
249 shared_ptr<BaseData> createData() const { return impl.createDataImpl(); }
250
251private:
252 Impl impl;
253};
254
255#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
256extern template struct FiniteDifferenceHelper<context::Scalar>;
257extern template struct DynamicsFiniteDifferenceHelper<context::Scalar>;
258#endif
259
260} // namespace autodiff
261} // namespace aligator
Base definitions for ternary functions.
#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
Definition math.hpp:8
::aligator::context::Scalar Scalar
Definition context.hpp:14
Main package namespace.
constexpr auto data(C &c) noexcept(noexcept(c.data())) -> decltype(c.data())
Definition data.hpp:18
const Manifold & space() const
State space for the input.
Definition dynamics.hpp:32
const int nu
Control dimension.
Definition dynamics.hpp:27
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)
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
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...
internal::finite_difference_impl< Scalar, StageFunctionTpl > Impl
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.