crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
rk4.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
10#define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
11
12#include "crocoddyl/core/fwd.hpp"
13#include "crocoddyl/core/integ-action-base.hpp"
14#include "crocoddyl/core/utils/deprecate.hpp"
15
16namespace crocoddyl {
17
31template <typename _Scalar>
33 public:
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35
36 typedef _Scalar Scalar;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
45
54 DEPRECATED("Use IntegratedActionModelRK",
55 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
56 boost::shared_ptr<ControlParametrizationModelAbstract> control,
57 const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);)
58
68 DEPRECATED("Use IntegratedActionModelRK",
69 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
70 const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);)
72
80 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
81 const Eigen::Ref<const VectorXs>& u);
82
92 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
93
101 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
102 const Eigen::Ref<const VectorXs>& u);
103
113 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
114
120 virtual boost::shared_ptr<ActionDataAbstract> createData();
121
125 virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
126
139 virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
140 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
141 const Scalar tol = Scalar(1e-9));
142
148 virtual void print(std::ostream& os) const;
149
150 protected:
151 using Base::control_;
152 using Base::differential_;
153 using Base::nu_;
154 using Base::state_;
155 using Base::time_step2_;
156 using Base::time_step_;
158
159 private:
160 std::array<Scalar, 4> rk4_c_;
161};
162
163template <typename _Scalar>
165 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166
167 typedef _Scalar Scalar;
172 typedef typename MathBase::VectorXs VectorXs;
173 typedef typename MathBase::MatrixXs MatrixXs;
174
175 template <template <typename Scalar> class Model>
176 explicit IntegratedActionDataRK4Tpl(Model<Scalar>* const model)
177 : Base(model),
178 integral(4, Scalar(0.)),
179 dx(model->get_state()->get_ndx()),
180 ki(4, VectorXs::Zero(model->get_state()->get_ndx())),
181 y(4, VectorXs::Zero(model->get_state()->get_nx())),
182 ws(4, VectorXs::Zero(model->get_control()->get_nw())),
183 dx_rk4(4, VectorXs::Zero(model->get_state()->get_ndx())),
184 dki_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
185 dki_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
186 dyi_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
187 dyi_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
188 dli_dx(4, VectorXs::Zero(model->get_state()->get_ndx())),
189 dli_du(4, VectorXs::Zero(model->get_nu())),
190 ddli_ddx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
191 ddli_ddw(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_control()->get_nw())),
192 ddli_ddu(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
193 ddli_dxdw(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_control()->get_nw())),
194 ddli_dxdu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
195 ddli_dwdu(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
196 Luu_partialx(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
197 Lxu_i(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
198 Lxx_partialx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
199 Lxx_partialu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
200 dx.setZero();
201
202 for (std::size_t i = 0; i < 4; ++i) {
203 differential.push_back(
204 boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
205 control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
206 }
207
208 const std::size_t nv = model->get_state()->get_nv();
209 dyi_dx[0].diagonal().setOnes();
210 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
211 }
212 virtual ~IntegratedActionDataRK4Tpl() {}
213
214 std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > differential;
215 std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
217 std::vector<Scalar> integral;
218 VectorXs dx;
219 std::vector<VectorXs> ki;
220 std::vector<VectorXs> y;
221 std::vector<VectorXs> ws;
222 std::vector<VectorXs> dx_rk4;
223
224 std::vector<MatrixXs>
226 std::vector<MatrixXs> dki_du;
228
229 std::vector<MatrixXs>
231 std::vector<MatrixXs> dyi_du;
233
234 std::vector<VectorXs>
236 std::vector<VectorXs> dli_du;
238
239 std::vector<MatrixXs> ddli_ddx;
241 std::vector<MatrixXs> ddli_ddw;
243 std::vector<MatrixXs> ddli_ddu;
245 std::vector<MatrixXs> ddli_dxdw;
247 std::vector<MatrixXs> ddli_dxdu;
249 std::vector<MatrixXs> ddli_dwdu;
251
252 std::vector<MatrixXs> Luu_partialx;
253 std::vector<MatrixXs> Lxu_i;
254 std::vector<MatrixXs> Lxx_partialx;
255 std::vector<MatrixXs> Lxx_partialu;
256
257 using Base::cost;
258 using Base::Fu;
259 using Base::Fx;
260 using Base::Lu;
261 using Base::Luu;
262 using Base::Lx;
263 using Base::Lxu;
264 using Base::Lxx;
265 using Base::r;
266 using Base::xnext;
267};
268
269} // namespace crocoddyl
270
271/* --- Details -------------------------------------------------------------- */
272/* --- Details -------------------------------------------------------------- */
273/* --- Details -------------------------------------------------------------- */
274#include "crocoddyl/core/integrator/rk4.hxx"
275
276#endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
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.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
boost::shared_ptr< StateAbstract > state_
< Dimension of the control
std::size_t nu_
< Dimension of the cost residual
Scalar time_step2_
Square of the time step used for integration.
Standard RK4 integrator.
Definition: rk4.hpp:32
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Integrate the differential action model using RK4 scheme.
DEPRECATED("Use IntegratedActionModelRK", IntegratedActionModelRK4Tpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true);) DEPRECATED("Use IntegratedActionModelRK"
Initialize the RK4 integrator.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the partial derivatives of the RK4 integrator.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator data.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Integrate the total cost value for nodes that depends only on the state using RK4 scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
virtual void quasiStatic(const boost::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))
Computes the quasic static commands.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
std::vector< MatrixXs > ddli_ddx
Definition: rk4.hpp:239
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
Definition: rk4.hpp:219
std::vector< MatrixXs > dki_du
Definition: rk4.hpp:226
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK4 nodes with respect to the state of the RK4 integration....
Definition: rk4.hpp:225
std::vector< MatrixXs > ddli_ddu
Definition: rk4.hpp:243
std::vector< MatrixXs > ddli_dxdw
Definition: rk4.hpp:245
std::vector< MatrixXs > ddli_ddw
Definition: rk4.hpp:241
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk4.hpp:214
std::vector< MatrixXs > ddli_dwdu
Definition: rk4.hpp:249
std::vector< MatrixXs > dyi_du
Definition: rk4.hpp:231
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
Definition: rk4.hpp:220
std::vector< VectorXs > dli_du
Definition: rk4.hpp:236
std::vector< MatrixXs > ddli_dxdu
Definition: rk4.hpp:247
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK4 integration....
Definition: rk4.hpp:235
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk4.hpp:216
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK4 dynamics with respect to the state of the RK4 integrator....
Definition: rk4.hpp:230
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
Definition: rk4.hpp:221