Crocoddyl
 
Loading...
Searching...
No Matches
diff-action.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// New York University, Max Planck Gesellschaft
6// Heriot-Watt University
7// Copyright note valid unless otherwise stated in individual files.
8// All rights reserved.
10
11#ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
12#define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
13
14#include "crocoddyl/core/diff-action-base.hpp"
15
16namespace crocoddyl {
17
43template <typename _Scalar>
45 : public DifferentialActionModelAbstractTpl<_Scalar> {
46 public:
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
50
51 typedef _Scalar Scalar;
57 typedef typename MathBase::VectorXs VectorXs;
58 typedef typename MathBase::MatrixXs MatrixXs;
59
69 std::shared_ptr<Base> model, const bool with_gauss_approx = false);
70 virtual ~DifferentialActionModelNumDiffTpl() = default;
71
75 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& u) override;
78
84 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) override;
86
90 virtual void calcDiff(
91 const std::shared_ptr<DifferentialActionDataAbstract>& data,
92 const Eigen::Ref<const VectorXs>& x,
93 const Eigen::Ref<const VectorXs>& u) override;
94
100 virtual void calcDiff(
101 const std::shared_ptr<DifferentialActionDataAbstract>& data,
102 const Eigen::Ref<const VectorXs>& x) override;
103
107 virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
108
118 template <typename NewScalar>
120
124 virtual void quasiStatic(
125 const std::shared_ptr<DifferentialActionDataAbstract>& data,
126 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
127 const std::size_t maxiter = 100,
128 const Scalar tol = Scalar(1e-9)) override;
129
134 const std::shared_ptr<Base>& get_model() const;
135
140 const Scalar get_disturbance() const;
141
146 void set_disturbance(const Scalar disturbance);
147
152
158 virtual void print(std::ostream& os) const override;
159
160 protected:
163 using Base::nr_;
164 using Base::nu_;
165 using Base::state_;
166 using Base::u_lb_;
167 using Base::u_ub_;
168
169 private:
170 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
171 std::shared_ptr<Base> model_;
172 bool with_gauss_approx_;
173 Scalar e_jac_;
175 Scalar e_hess_;
177};
178
179template <typename _Scalar>
181 : public DifferentialActionDataAbstractTpl<_Scalar> {
182 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
183
184 typedef _Scalar Scalar;
187 typedef typename MathBase::VectorXs VectorXs;
188 typedef typename MathBase::MatrixXs MatrixXs;
189
196 template <template <typename Scalar> class Model>
197 explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
198 : Base(model),
199 Rx(model->get_model()->get_nr(),
200 model->get_model()->get_state()->get_ndx()),
201 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
202 dx(model->get_model()->get_state()->get_ndx()),
203 du(model->get_model()->get_nu()),
204 xp(model->get_model()->get_state()->get_nx()) {
205 Rx.setZero();
206 Ru.setZero();
207 dx.setZero();
208 du.setZero();
209 xp.setZero();
210
211 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
212 const std::size_t nu = model->get_model()->get_nu();
213 data_0 = model->get_model()->createData();
214 for (std::size_t i = 0; i < ndx; ++i) {
215 data_x.push_back(model->get_model()->createData());
216 }
217 for (std::size_t i = 0; i < nu; ++i) {
218 data_u.push_back(model->get_model()->createData());
219 }
220 }
221
222 Scalar x_norm;
223 Scalar
225 Scalar
227 Scalar xh_hess;
229 Scalar uh_hess;
231 Scalar xh_hess_pow2;
232 Scalar uh_hess_pow2;
233 Scalar xuh_hess_pow2;
234 MatrixXs Rx;
235 MatrixXs Ru;
236 VectorXs dx;
237 VectorXs du;
238 VectorXs xp;
239 std::shared_ptr<Base> data_0;
240 std::vector<std::shared_ptr<Base> > data_x;
241 std::vector<std::shared_ptr<Base> > data_u;
242
243 using Base::cost;
244 using Base::Fu;
245 using Base::Fx;
246 using Base::g;
247 using Base::Gu;
248 using Base::Gx;
249 using Base::h;
250 using Base::Hu;
251 using Base::Hx;
252 using Base::Lu;
253 using Base::Luu;
254 using Base::Lx;
255 using Base::Lxu;
256 using Base::Lxx;
257 using Base::r;
258 using Base::xout;
259};
260
261} // namespace crocoddyl
262
263/* --- Details -------------------------------------------------------------- */
264/* --- Details -------------------------------------------------------------- */
265/* --- Details -------------------------------------------------------------- */
266#include "crocoddyl/core/numdiff/diff-action.hxx"
267
268CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
270CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
272
273#endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
Abstract class for differential action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of a differential action model.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
const std::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the system acceleration and cost value.
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.
virtual std::shared_ptr< DifferentialActionDataAbstract > createData() override
Create the differential action data.
DifferentialActionModelNumDiffTpl(std::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
DifferentialActionModelNumDiffTpl< NewScalar > cast() const
Cast the diff-action numdiff model to a different scalar type.
virtual void quasiStatic(const std::shared_ptr< DifferentialActionDataAbstract > &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 print(std::ostream &os) const override
Print relevant information of the action numdiff model.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics and cost functions.
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
VectorXs h
Equality constraint values.
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs g
Inequality constraint values.
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 .
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
Scalar x_norm
Norm of the state vector.
Scalar uh_jac
Disturbance value used for computing .
Scalar xh_jac
Disturbance value used for computing .