crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
action-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_ACTION_BASE_HPP_
10#define CROCODDYL_CORE_ACTION_BASE_HPP_
11
12#include <stdexcept>
13#include <boost/shared_ptr.hpp>
14#include <boost/make_shared.hpp>
15
16#include "crocoddyl/core/fwd.hpp"
17#include "crocoddyl/core/state-base.hpp"
18#include "crocoddyl/core/utils/math.hpp"
19
20namespace crocoddyl {
21
58template <typename _Scalar>
60 public:
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62
63 typedef _Scalar Scalar;
67 typedef typename MathBase::VectorXs VectorXs;
68
76 ActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu, const std::size_t nr = 0);
78
86 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
87 const Eigen::Ref<const VectorXs>& u) = 0;
88
98 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
99
111 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
112 const Eigen::Ref<const VectorXs>& u) = 0;
113
123 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
124
130 virtual boost::shared_ptr<ActionDataAbstract> createData();
131
135 virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
136
149 virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
150 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
151 const Scalar tol = Scalar(1e-9));
152
164 VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
165 const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
166
170 std::size_t get_nu() const;
171
175 std::size_t get_nr() const;
176
180 const boost::shared_ptr<StateAbstract>& get_state() const;
181
185 const VectorXs& get_u_lb() const;
186
190 const VectorXs& get_u_ub() const;
191
196
200 void set_u_lb(const VectorXs& u_lb);
201
205 void set_u_ub(const VectorXs& u_ub);
206
210 template <class Scalar>
211 friend std::ostream& operator<<(std::ostream& os, const ActionModelAbstractTpl<Scalar>& model);
212
218 virtual void print(std::ostream& os) const;
219
220 protected:
221 std::size_t nu_;
222 std::size_t nr_;
223 boost::shared_ptr<StateAbstract> state_;
224 VectorXs unone_;
225 VectorXs u_lb_;
226 VectorXs u_ub_;
228
233};
234
235template <typename _Scalar>
237 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
238
239 typedef _Scalar Scalar;
241 typedef typename MathBase::VectorXs VectorXs;
242 typedef typename MathBase::MatrixXs MatrixXs;
243
244 template <template <typename Scalar> class Model>
245 explicit ActionDataAbstractTpl(Model<Scalar>* const model)
246 : cost(Scalar(0.)),
247 xnext(model->get_state()->get_nx()),
248 Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
249 Fu(model->get_state()->get_ndx(), model->get_nu()),
250 r(model->get_nr()),
251 Lx(model->get_state()->get_ndx()),
252 Lu(model->get_nu()),
253 Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
254 Lxu(model->get_state()->get_ndx(), model->get_nu()),
255 Luu(model->get_nu(), model->get_nu()) {
256 xnext.setZero();
257 Fx.setZero();
258 Fu.setZero();
259 r.setZero();
260 Lx.setZero();
261 Lu.setZero();
262 Lxx.setZero();
263 Lxu.setZero();
264 Luu.setZero();
265 }
266 virtual ~ActionDataAbstractTpl() {}
267
268 Scalar cost;
269 VectorXs xnext;
270 MatrixXs Fx;
271 MatrixXs Fu;
272 VectorXs r;
273 VectorXs Lx;
274 VectorXs Lu;
275 MatrixXs Lxx;
276 MatrixXs Lxu;
277 MatrixXs Luu;
278};
279
280} // namespace crocoddyl
281
282/* --- Details -------------------------------------------------------------- */
283/* --- Details -------------------------------------------------------------- */
284/* --- Details -------------------------------------------------------------- */
285#include "crocoddyl/core/action-base.hxx"
286
287#endif // CROCODDYL_CORE_ACTION_BASE_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
const VectorXs & get_u_ub() const
Return the control upper bound.
virtual void print(std::ostream &os) const
Print relevant information of the action model.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
bool has_control_limits_
Indicates whether any of the control limits is finite.
bool get_has_control_limits() const
Indicates if there are defined control limits.
VectorXs quasiStatic_x(const boost::shared_ptr< ActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
void update_has_control_limits()
Update the status of the control limits (i.e. if there are defined limits)
ActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
Initialize the action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
std::size_t nr_
Dimension of the cost residual.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the derivatives of the dynamics and cost functions.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
const VectorXs & get_u_lb() const
Return the control lower bound.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the next state and cost value.
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
friend std::ostream & operator<<(std::ostream &os, const ActionModelAbstractTpl< Scalar > &model)
Print information on the action model.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
std::size_t get_nu() const
Return the dimension of the control input.
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.
Abstract class for the state representation.
Definition: state-base.hpp:42
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.