Crocoddyl
 
Loading...
Searching...
No Matches
action.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5// Max Planck Gesellschaft, University of Edinburgh,
6// Heriot-Watt University
7// Copyright note valid unless otherwise stated in individual files.
8// All rights reserved.
10
11#ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
12#define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
13
14#include "crocoddyl/core/action-base.hpp"
15#include "crocoddyl/core/fwd.hpp"
16
17namespace crocoddyl {
18
43template <typename _Scalar>
45 public:
46 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 CROCODDYL_DERIVED_CAST(ActionModelBase, ActionModelNumDiffTpl)
48
49 typedef _Scalar Scalar;
54 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
55 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
56
65 explicit ActionModelNumDiffTpl(std::shared_ptr<Base> model,
66 bool with_gauss_approx = false);
67 virtual ~ActionModelNumDiffTpl() = default;
68
72 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>& x,
74 const Eigen::Ref<const VectorXs>& u) override;
75
80 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x) override;
82
86 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& u) override;
89
94 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x) override;
96
100 virtual std::shared_ptr<ActionDataAbstract> createData() override;
101
105 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
106 Eigen::Ref<VectorXs> u,
107 const Eigen::Ref<const VectorXs>& x,
108 const std::size_t maxiter = 100,
109 const Scalar tol = Scalar(1e-9)) override;
110
120 template <typename NewScalar>
122
126 const std::shared_ptr<Base>& get_model() const;
127
132 const Scalar get_disturbance() const;
133
138 void set_disturbance(const Scalar disturbance);
139
144
150 virtual void print(std::ostream& os) const override;
151
152 protected:
155 using Base::nr_;
156 using Base::nu_;
157 using Base::state_;
158 using Base::u_lb_;
159 using Base::u_ub_;
160
161 private:
173 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
174
175 std::shared_ptr<Base> model_;
177 Scalar e_jac_;
179 Scalar e_hess_;
181 bool with_gauss_approx_;
183};
184
185template <typename _Scalar>
187 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188
189 typedef _Scalar Scalar;
192 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
193 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
194
201 template <template <typename Scalar> class Model>
202 explicit ActionDataNumDiffTpl(Model<Scalar>* const model)
203 : Base(model),
204 Rx(model->get_model()->get_nr(),
205 model->get_model()->get_state()->get_ndx()),
206 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
207 dx(model->get_model()->get_state()->get_ndx()),
208 du(model->get_model()->get_nu()),
209 xp(model->get_model()->get_state()->get_nx()) {
210 Rx.setZero();
211 Ru.setZero();
212 dx.setZero();
213 du.setZero();
214 xp.setZero();
215
216 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
217 const std::size_t nu = model->get_model()->get_nu();
218 data_0 = model->get_model()->createData();
219 for (std::size_t i = 0; i < ndx; ++i) {
220 data_x.push_back(model->get_model()->createData());
221 }
222 for (std::size_t i = 0; i < nu; ++i) {
223 data_u.push_back(model->get_model()->createData());
224 }
225 }
226
227 using Base::cost;
228 using Base::Fu;
229 using Base::Fx;
230 using Base::Lu;
231 using Base::Luu;
232 using Base::Lx;
233 using Base::Lxu;
234 using Base::Lxx;
235 using Base::r;
236 using Base::xnext;
237
238 Scalar x_norm;
239 Scalar
241 Scalar
243 Scalar xh_hess;
245 Scalar uh_hess;
247 Scalar xh_hess_pow2;
248 Scalar uh_hess_pow2;
249 Scalar xuh_hess_pow2;
250 MatrixXs Rx;
251 MatrixXs Ru;
252 VectorXs dx;
253 VectorXs du;
254 VectorXs xp;
256 std::shared_ptr<Base> data_0;
257 std::vector<std::shared_ptr<Base> >
259 std::vector<std::shared_ptr<Base> >
261};
262
263} // namespace crocoddyl
264
265/* --- Details -------------------------------------------------------------- */
266/* --- Details -------------------------------------------------------------- */
267/* --- Details -------------------------------------------------------------- */
268#include "crocoddyl/core/numdiff/action.hxx"
269
270CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActionModelNumDiffTpl)
271CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActionDataNumDiffTpl)
272
273#endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
Abstract class for action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
std::size_t nu_
Control dimension.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of an action model.
Definition action.hpp:44
ActionModelNumDiffTpl< NewScalar > cast() const
Cast the action numdiff model to a different scalar type.
const std::shared_ptr< Base > & get_model() const
Return the acton model that we use to numerical differentiate.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the next state and cost value.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the action data.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
const Scalar get_disturbance() const
Return the disturbance constant used in the numerical differentiation routine.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used in the numerical differentiation routine.
ActionModelNumDiffTpl(std::shared_ptr< Base > model, bool with_gauss_approx=false)
Initialize the numdiff action model.
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
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics and cost functions.
virtual void print(std::ostream &os) const override
Print relevant information of the diff-action numdiff model.
VectorXs xnext
evolution state
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 .
Scalar x_norm
Norm of the state vector.
Definition action.hpp:238
MatrixXs Ru
Cost residual jacobian: .
Definition action.hpp:251
MatrixXs Rx
Cost residual jacobian: .
Definition action.hpp:250
std::vector< std::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition action.hpp:258
Scalar uh_jac
Disturbance value used for computing .
Definition action.hpp:242
std::vector< std::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition action.hpp:260
Scalar xh_jac
Disturbance value used for computing .
Definition action.hpp:240
VectorXs du
Control disturbance.
Definition action.hpp:253
std::shared_ptr< Base > data_0
The data that contains the final results.
Definition action.hpp:256
ActionDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff action data.
Definition action.hpp:202
VectorXs dx
State disturbance.
Definition action.hpp:252