crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
rk.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, University of Edinburgh, University of Trento,
5// LAAS-CNRS, IRI: CSIC-UPC
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
34template <typename _Scalar>
36 public:
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38
39 typedef _Scalar Scalar;
46 typedef typename MathBase::VectorXs VectorXs;
47 typedef typename MathBase::MatrixXs MatrixXs;
48
58 IntegratedActionModelRKTpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
59 boost::shared_ptr<ControlParametrizationModelAbstract> control, const RKType rktype,
60 const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
61
72 IntegratedActionModelRKTpl(boost::shared_ptr<DifferentialActionModelAbstract> model, const RKType rktype,
73 const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
75
83 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
84 const Eigen::Ref<const VectorXs>& u);
85
95 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
96
104 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
105 const Eigen::Ref<const VectorXs>& u);
106
116 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
117
123 virtual boost::shared_ptr<ActionDataAbstract> createData();
124
128 virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
129
142 virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
143 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
144 const Scalar tol = Scalar(1e-9));
145
149 std::size_t get_ni() const;
150
156 virtual void print(std::ostream& os) const;
157
158 protected:
159 using Base::control_;
160 using Base::differential_;
161 using Base::nu_;
162 using Base::state_;
163 using Base::time_step2_;
164 using Base::time_step_;
166
167 private:
171 void set_rk_type(const RKType rktype);
172
173 std::vector<Scalar> rk_c_;
174 std::size_t ni_;
175};
176
177template <typename _Scalar>
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180
181 typedef _Scalar Scalar;
186 typedef typename MathBase::VectorXs VectorXs;
187 typedef typename MathBase::MatrixXs MatrixXs;
188
189 template <template <typename Scalar> class Model>
190 explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
191 : Base(model),
192 integral(model->get_ni(), Scalar(0.)),
193 dx(model->get_state()->get_ndx()),
194 ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
195 y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
196 ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
197 dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
198 dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
199 dki_du(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
200 dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
201 dyi_du(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
202 dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
203 dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
204 ddli_ddx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
205 ddli_ddw(model->get_ni(), MatrixXs::Zero(model->get_control()->get_nw(), model->get_control()->get_nw())),
206 ddli_ddu(model->get_ni(), MatrixXs::Zero(model->get_nu(), model->get_nu())),
207 ddli_dxdw(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_control()->get_nw())),
208 ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
209 ddli_dwdu(model->get_ni(), MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
210 Luu_partialx(model->get_ni(), MatrixXs::Zero(model->get_nu(), model->get_nu())),
211 Lxu_i(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
212 Lxx_partialx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
213 Lxx_partialu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
214 dx.setZero();
215
216 for (std::size_t i = 0; i < model->get_ni(); ++i) {
217 differential.push_back(
218 boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
219 control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
220 }
221
222 const std::size_t nv = model->get_state()->get_nv();
223 dyi_dx[0].diagonal().setOnes();
224 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
225 }
226 virtual ~IntegratedActionDataRKTpl() {}
227
228 std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > differential;
229 std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
231 std::vector<Scalar> integral;
232 VectorXs dx;
233 std::vector<VectorXs> ki;
234 std::vector<VectorXs> y;
235 std::vector<VectorXs> ws;
236 std::vector<VectorXs> dx_rk;
237
238 std::vector<MatrixXs>
240 std::vector<MatrixXs> dki_du;
242
243 std::vector<MatrixXs>
245 std::vector<MatrixXs> dyi_du;
247
248 std::vector<VectorXs>
250 std::vector<VectorXs> dli_du;
252
253 std::vector<MatrixXs> ddli_ddx;
255 std::vector<MatrixXs> ddli_ddw;
257 std::vector<MatrixXs> ddli_ddu;
259 std::vector<MatrixXs> ddli_dxdw;
261 std::vector<MatrixXs> ddli_dxdu;
263 std::vector<MatrixXs> ddli_dwdu;
265
266 std::vector<MatrixXs> Luu_partialx;
267 std::vector<MatrixXs> Lxu_i;
268 std::vector<MatrixXs> Lxx_partialx;
269 std::vector<MatrixXs> Lxx_partialu;
270
271 using Base::cost;
272 using Base::Fu;
273 using Base::Fx;
274 using Base::Lu;
275 using Base::Luu;
276 using Base::Lx;
277 using Base::Lxu;
278 using Base::Lxx;
279 using Base::r;
280 using Base::xnext;
281};
282
283} // namespace crocoddyl
284
285/* --- Details -------------------------------------------------------------- */
286/* --- Details -------------------------------------------------------------- */
287/* --- Details -------------------------------------------------------------- */
288#include "crocoddyl/core/integrator/rk.hxx"
289
290#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 RK integrator.
Definition: rk.hpp:35
IntegratedActionModelRKTpl(boost::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 print(std::ostream &os) const
Print relevant information of the RK integrator model.
IntegratedActionModelRKTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::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 boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Integrate the differential action model using RK scheme.
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 RK integrator.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK 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 RK scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
std::size_t get_ni() const
Return the number of nodes of the integrator.
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: rk.hpp:253
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition: rk.hpp:233
std::vector< MatrixXs > dki_du
Definition: rk.hpp:240
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK nodes with respect to the state of the RK integration....
Definition: rk.hpp:239
std::vector< MatrixXs > ddli_ddu
Definition: rk.hpp:257
std::vector< MatrixXs > ddli_dxdw
Definition: rk.hpp:259
std::vector< MatrixXs > ddli_ddw
Definition: rk.hpp:255
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk.hpp:228
std::vector< MatrixXs > ddli_dwdu
Definition: rk.hpp:263
std::vector< MatrixXs > dyi_du
Definition: rk.hpp:245
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition: rk.hpp:234
std::vector< VectorXs > dli_du
Definition: rk.hpp:250
std::vector< MatrixXs > ddli_dxdu
Definition: rk.hpp:261
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK integration....
Definition: rk.hpp:249
VectorXs dx
State rate.
Definition: rk.hpp:232
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk.hpp:230
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK dynamics with respect to the state of the RK integrator....
Definition: rk.hpp:244
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition: rk.hpp:235