crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
action.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, New York University,
5// Max Planck Gesellschaft, University of Edinburgh
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
11#define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
12
13#include <vector>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/action-base.hpp"
17
18namespace crocoddyl {
19
41template <typename _Scalar>
43 public:
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45
46 typedef _Scalar Scalar;
51 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
52 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
53
60 explicit ActionModelNumDiffTpl(boost::shared_ptr<Base> model, bool with_gauss_approx = false);
61 virtual ~ActionModelNumDiffTpl();
62
66 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
67 const Eigen::Ref<const VectorXs>& u);
68
72 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
73
77 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
78 const Eigen::Ref<const VectorXs>& u);
79
84 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
85
89 virtual boost::shared_ptr<ActionDataAbstract> createData();
90
94 const boost::shared_ptr<Base>& get_model() const;
95
99 const Scalar get_disturbance() const;
100
104 void set_disturbance(const Scalar disturbance);
105
110
111 protected:
113 using Base::nr_;
114 using Base::nu_;
115 using Base::state_;
116 using Base::u_lb_;
117 using Base::u_ub_;
118 using Base::unone_;
119
120 private:
132 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
133
134 boost::shared_ptr<Base> model_;
135 Scalar disturbance_;
136 bool with_gauss_approx_;
137};
138
139template <typename _Scalar>
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142
143 typedef _Scalar Scalar;
146 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
147 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
148
155 template <template <typename Scalar> class Model>
156 explicit ActionDataNumDiffTpl(Model<Scalar>* const model)
157 : Base(model),
158 Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
159 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
160 dx(model->get_model()->get_state()->get_ndx()),
161 du(model->get_model()->get_nu()),
162 xp(model->get_model()->get_state()->get_nx()) {
163 Rx.setZero();
164 Ru.setZero();
165 dx.setZero();
166 du.setZero();
167 xp.setZero();
168
169 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
170 const std::size_t nu = model->get_model()->get_nu();
171 data_0 = model->get_model()->createData();
172 for (std::size_t i = 0; i < ndx; ++i) {
173 data_x.push_back(model->get_model()->createData());
174 }
175 for (std::size_t i = 0; i < nu; ++i) {
176 data_u.push_back(model->get_model()->createData());
177 }
178 }
179
180 using Base::cost;
181 using Base::Fu;
182 using Base::Fx;
183 using Base::Lu;
184 using Base::Luu;
185 using Base::Lx;
186 using Base::Lxu;
187 using Base::Lxx;
188 using Base::r;
189 using Base::xnext;
190
191 MatrixXs Rx;
192 MatrixXs Ru;
193 VectorXs dx;
194 VectorXs du;
195 VectorXs xp;
196 boost::shared_ptr<Base> data_0;
197 std::vector<boost::shared_ptr<Base> > data_x;
198 std::vector<boost::shared_ptr<Base> > data_u;
199};
200
201} // namespace crocoddyl
202
203/* --- Details -------------------------------------------------------------- */
204/* --- Details -------------------------------------------------------------- */
205/* --- Details -------------------------------------------------------------- */
206#include "crocoddyl/core/numdiff/action.hxx"
207
208#endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of an action model.
Definition: action.hpp:42
const boost::shared_ptr< Base > & get_model() const
Return the acton model that we use to numerical differentiate.
const Scalar get_disturbance() const
Return the disturbance 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 used in the numerical differentiation routine.
ActionModelNumDiffTpl(boost::shared_ptr< Base > model, bool with_gauss_approx=false)
Initialize the numdiff action model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
VectorXs xnext
evolution state
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.
VectorXs r
Cost residual.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: action.hpp:198
MatrixXs Ru
Cost residual jacobian: .
Definition: action.hpp:192
MatrixXs Rx
Cost residual jacobian: .
Definition: action.hpp:191
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: action.hpp:196
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: action.hpp:195
VectorXs du
Control disturbance.
Definition: action.hpp:194
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: action.hpp:197
ActionDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff action data.
Definition: action.hpp:156
VectorXs dx
State disturbance.
Definition: action.hpp:193