Crocoddyl
 
Loading...
Searching...
No Matches
euler.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// University of Oxford, Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
11#define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
12
13#include "crocoddyl/core/fwd.hpp"
14#include "crocoddyl/core/integ-action-base.hpp"
15
16namespace crocoddyl {
17
36template <typename _Scalar>
38 : public IntegratedActionModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelEulerTpl)
42
43 typedef _Scalar Scalar;
54 typedef typename MathBase::VectorXs VectorXs;
55 typedef typename MathBase::MatrixXs MatrixXs;
56
66 std::shared_ptr<DifferentialActionModelAbstract> model,
67 std::shared_ptr<ControlParametrizationModelAbstract> control,
68 const Scalar time_step = Scalar(1e-3),
69 const bool with_cost_residual = true);
70
82 std::shared_ptr<DifferentialActionModelAbstract> model,
83 const Scalar time_step = Scalar(1e-3),
84 const bool with_cost_residual = true);
85 virtual ~IntegratedActionModelEulerTpl() = default;
86
95 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u) override;
98
109 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x) override;
111
119 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
120 const Eigen::Ref<const VectorXs>& x,
121 const Eigen::Ref<const VectorXs>& u) override;
122
133 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
134 const Eigen::Ref<const VectorXs>& x) override;
135
141 virtual std::shared_ptr<ActionDataAbstract> createData() override;
142
152 template <typename NewScalar>
154
158 virtual bool checkData(
159 const std::shared_ptr<ActionDataAbstract>& data) override;
160
174 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
175 Eigen::Ref<VectorXs> u,
176 const Eigen::Ref<const VectorXs>& x,
177 const std::size_t maxiter = 100,
178 const Scalar tol = Scalar(1e-9)) override;
179
185 virtual void print(std::ostream& os) const override;
186
187 protected:
188 using Base::control_;
189 using Base::differential_;
190 using Base::ng_;
191 using Base::nh_;
192 using Base::nu_;
193 using Base::state_;
194 using Base::time_step2_;
195 using Base::time_step_;
198};
199
200template <typename _Scalar>
202 : public IntegratedActionDataAbstractTpl<_Scalar> {
203 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204
205 typedef _Scalar Scalar;
212 typedef typename MathBase::VectorXs VectorXs;
213 typedef typename MathBase::MatrixXs MatrixXs;
214
215 template <template <typename Scalar> class Model>
216 explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model)
217 : Base(model) {
218 differential = model->get_differential()->createData();
219 control = model->get_control()->createData();
220 const std::size_t ndx = model->get_state()->get_ndx();
221 const std::size_t nv = model->get_state()->get_nv();
222 dx = VectorXs::Zero(ndx);
223 da_du = MatrixXs::Zero(nv, model->get_nu());
224 Lwu = MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu());
225 }
226 virtual ~IntegratedActionDataEulerTpl() = default;
227
228 std::shared_ptr<DifferentialActionDataAbstract>
230 std::shared_ptr<ControlParametrizationDataAbstract>
232 VectorXs dx;
233 MatrixXs da_du;
234 MatrixXs Lwu;
236
237 using Base::cost;
238 using Base::Fu;
239 using Base::Fx;
240 using Base::Lu;
241 using Base::Luu;
242 using Base::Lx;
243 using Base::Lxu;
244 using Base::Lxx;
245 using Base::r;
246 using Base::xnext;
247};
248
249} // namespace crocoddyl
250
251/* --- Details -------------------------------------------------------------- */
252/* --- Details -------------------------------------------------------------- */
253/* --- Details -------------------------------------------------------------- */
254#include "crocoddyl/core/integrator/euler.hxx"
255
256CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
258CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
260
261#endif // CROCODDYL_CORE_INTEGRATOR_EULER_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.
Symplectic Euler integrator.
Definition euler.hpp:38
IntegratedActionModelEulerTpl< NewScalar > cast() const
Cast the Euler integrated-action model to a different scalar type.
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 symplectic Euler scheme.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the symplectic Euler data.
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.
IntegratedActionModelEulerTpl(std::shared_ptr< DifferentialActionModelAbstract > model, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the symplectic Euler 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.
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 symplectic Euler scheme...
IntegratedActionModelEulerTpl(std::shared_ptr< DifferentialActionModelAbstract > model, std::shared_ptr< ControlParametrizationModelAbstract > control, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the symplectic Euler integrator.
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 symplectic Euler integrator.
virtual void print(std::ostream &os) const override
Print relevant information of the Euler 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::shared_ptr< ControlParametrizationDataAbstract > control
Control parametrization data.
Definition euler.hpp:231
std::shared_ptr< DifferentialActionDataAbstract > differential
Differential model data.
Definition euler.hpp:229