Crocoddyl
 
Loading...
Searching...
No Matches
rk.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, University of Trento,
5// LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_INTEGRATOR_RK_HPP_
11#define CROCODDYL_CORE_INTEGRATOR_RK_HPP_
12
13#include "crocoddyl/core/fwd.hpp"
14#include "crocoddyl/core/integ-action-base.hpp"
15
16namespace crocoddyl {
17
18enum RKType { two = 2, three = 3, four = 4 };
19
38template <typename _Scalar>
40 : public IntegratedActionModelAbstractTpl<_Scalar> {
41 public:
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelRKTpl)
44
45 typedef _Scalar Scalar;
58 typedef typename MathBase::VectorXs VectorXs;
59 typedef typename MathBase::MatrixXs MatrixXs;
60
71 std::shared_ptr<DifferentialActionModelAbstract> model,
72 std::shared_ptr<ControlParametrizationModelAbstract> control,
73 const RKType rktype, const Scalar time_step = Scalar(1e-3),
74 const bool with_cost_residual = true);
75
88 std::shared_ptr<DifferentialActionModelAbstract> model,
89 const RKType rktype, const Scalar time_step = Scalar(1e-3),
90 const bool with_cost_residual = true);
91 virtual ~IntegratedActionModelRKTpl() = default;
92
100 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
101 const Eigen::Ref<const VectorXs>& x,
102 const Eigen::Ref<const VectorXs>& u) override;
103
114 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
115 const Eigen::Ref<const VectorXs>& x) override;
116
124 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
125 const Eigen::Ref<const VectorXs>& x,
126 const Eigen::Ref<const VectorXs>& u) override;
127
138 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
139 const Eigen::Ref<const VectorXs>& x) override;
140
146 virtual std::shared_ptr<ActionDataAbstract> createData() override;
147
157 template <typename NewScalar>
159
163 virtual bool checkData(
164 const std::shared_ptr<ActionDataAbstract>& data) override;
165
179 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
180 Eigen::Ref<VectorXs> u,
181 const Eigen::Ref<const VectorXs>& x,
182 const std::size_t maxiter = 100,
183 const Scalar tol = Scalar(1e-9)) override;
184
188 std::size_t get_ni() const;
189
195 virtual void print(std::ostream& os) const override;
196
197 protected:
198 using Base::control_;
199 using Base::differential_;
200 using Base::ng_;
201 using Base::nh_;
202 using Base::nu_;
203 using Base::state_;
204 using Base::time_step2_;
205 using Base::time_step_;
208
209 private:
213 void set_rk_type(const RKType rktype);
214
215 RKType rk_type_;
216 std::vector<Scalar> rk_c_;
217 std::size_t ni_;
218};
219
220template <typename _Scalar>
222 : public IntegratedActionDataAbstractTpl<_Scalar> {
223 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
224
225 typedef _Scalar Scalar;
232 typedef typename MathBase::VectorXs VectorXs;
233 typedef typename MathBase::MatrixXs MatrixXs;
234
235 template <template <typename Scalar> class Model>
236 explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
237 : Base(model),
238 integral(model->get_ni(), Scalar(0.)),
239 dx(model->get_state()->get_ndx()),
240 ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
241 y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
242 ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
243 dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
244 dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
245 model->get_state()->get_ndx())),
246 dki_du(model->get_ni(),
247 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
248 dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
249 model->get_state()->get_ndx())),
250 dyi_du(model->get_ni(),
251 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
252 dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
253 dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
254 ddli_ddx(model->get_ni(),
255 MatrixXs::Zero(model->get_state()->get_ndx(),
256 model->get_state()->get_ndx())),
257 ddli_ddw(model->get_ni(),
258 MatrixXs::Zero(model->get_control()->get_nw(),
259 model->get_control()->get_nw())),
260 ddli_ddu(model->get_ni(),
261 MatrixXs::Zero(model->get_nu(), model->get_nu())),
262 ddli_dxdw(model->get_ni(),
263 MatrixXs::Zero(model->get_state()->get_ndx(),
264 model->get_control()->get_nw())),
265 ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
266 model->get_nu())),
267 ddli_dwdu(
268 model->get_ni(),
269 MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
270 Luu_partialx(model->get_ni(),
271 MatrixXs::Zero(model->get_nu(), model->get_nu())),
272 Lxu_i(model->get_ni(),
273 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
274 Lxx_partialx(model->get_ni(),
275 MatrixXs::Zero(model->get_state()->get_ndx(),
276 model->get_state()->get_ndx())),
277 Lxx_partialu(
278 model->get_ni(),
279 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
280 dx.setZero();
281
282 for (std::size_t i = 0; i < model->get_ni(); ++i) {
283 differential.push_back(std::shared_ptr<DifferentialActionDataAbstract>(
284 model->get_differential()->createData()));
285 control.push_back(std::shared_ptr<ControlParametrizationDataAbstract>(
286 model->get_control()->createData()));
287 }
288
289 const std::size_t nv = model->get_state()->get_nv();
290 dyi_dx[0].diagonal().setOnes();
291 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
292 }
293 virtual ~IntegratedActionDataRKTpl() = default;
294
295 std::vector<std::shared_ptr<DifferentialActionDataAbstract> >
297 std::vector<std::shared_ptr<ControlParametrizationDataAbstract> >
299 std::vector<Scalar> integral;
300 VectorXs dx;
301 std::vector<VectorXs> ki;
302 std::vector<VectorXs>
304 std::vector<VectorXs> ws;
305 std::vector<VectorXs> dx_rk;
306
307 std::vector<MatrixXs>
310 std::vector<MatrixXs>
313
314 std::vector<MatrixXs>
317 std::vector<MatrixXs>
320
321 std::vector<VectorXs>
324 std::vector<VectorXs>
327
328 std::vector<MatrixXs>
331 std::vector<MatrixXs>
335 std::vector<MatrixXs> ddli_ddu;
338 std::vector<MatrixXs>
342 std::vector<MatrixXs>
346 std::vector<MatrixXs>
350
351 std::vector<MatrixXs> Luu_partialx;
352 std::vector<MatrixXs> Lxu_i;
353 std::vector<MatrixXs> Lxx_partialx;
354 std::vector<MatrixXs> Lxx_partialu;
355
356 using Base::cost;
357 using Base::Fu;
358 using Base::Fx;
359 using Base::Lu;
360 using Base::Luu;
361 using Base::Lx;
362 using Base::Lxu;
363 using Base::Lxx;
364 using Base::r;
365 using Base::xnext;
366};
367
368} // namespace crocoddyl
369
370/* --- Details -------------------------------------------------------------- */
371/* --- Details -------------------------------------------------------------- */
372/* --- Details -------------------------------------------------------------- */
373#include "crocoddyl/core/integrator/rk.hxx"
374
375CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::IntegratedActionModelRKTpl)
376CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::IntegratedActionDataRKTpl)
377
378#endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
std::size_t nh_
Number of equality constraints.
std::size_t ng_
Number of inequality constraints.
Abstract class for the control trajectory parametrization.
Abstract class for differential action model.
Abstract class for an integrated action model.
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Scalar time_step_
Time step used for integration.
std::shared_ptr< StateAbstract > state_
< Dimension of the control
std::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
std::size_t nu_
< Dimension of the cost residual
std::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
Scalar time_step2_
Square of the time step used for integration.
Standard RK integrator.
Definition rk.hpp:40
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Integrate the differential action model using RK scheme.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the RK integrator data.
IntegratedActionModelRKTpl< NewScalar > cast() const
Cast the RK integrated-action model to a different scalar type.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the partial derivatives of the cost.
virtual bool checkData(const std::shared_ptr< ActionDataAbstract > &data) override
Checks that a specific data belongs to this model.
IntegratedActionModelRKTpl(std::shared_ptr< DifferentialActionModelAbstract > model, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
virtual void quasiStatic(const std::shared_ptr< ActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) override
Computes the quasic static commands.
IntegratedActionModelRKTpl(std::shared_ptr< DifferentialActionModelAbstract > model, std::shared_ptr< ControlParametrizationModelAbstract > control, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Integrate the total cost value for nodes that depends only on the state using RK scheme.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the partial derivatives of the RK integrator.
std::size_t get_ni() const
Return the number of nodes of the integrator.
virtual void print(std::ostream &os) const override
Print relevant information of the RK integrator model.
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
std::vector< MatrixXs > ddli_ddx
Definition rk.hpp:329
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition rk.hpp:301
std::vector< MatrixXs > dki_du
Definition rk.hpp:311
std::vector< std::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition rk.hpp:296
std::vector< MatrixXs > dki_dx
Definition rk.hpp:308
std::vector< MatrixXs > ddli_ddu
Definition rk.hpp:335
std::vector< MatrixXs > ddli_dxdw
Definition rk.hpp:339
std::vector< MatrixXs > ddli_ddw
Definition rk.hpp:332
std::vector< MatrixXs > ddli_dwdu
Definition rk.hpp:347
std::vector< MatrixXs > dyi_du
Definition rk.hpp:318
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition rk.hpp:303
std::vector< VectorXs > dli_du
Definition rk.hpp:325
std::vector< std::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition rk.hpp:298
std::vector< MatrixXs > ddli_dxdu
Definition rk.hpp:343
std::vector< VectorXs > dli_dx
Definition rk.hpp:322
std::vector< MatrixXs > dyi_dx
Definition rk.hpp:315
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition rk.hpp:304