crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
unicycle.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
10 #define CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
11 
12 #include <stdexcept>
13 
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/action-base.hpp"
16 #include "crocoddyl/core/states/euclidean.hpp"
17 
18 namespace crocoddyl {
19 template <typename _Scalar>
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::Vector2s Vector2s;
31 
33  virtual ~ActionModelUnicycleTpl();
34 
35  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
36  const Eigen::Ref<const VectorXs>& u);
37  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
38  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
39  const Eigen::Ref<const VectorXs>& u);
40  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
41  virtual boost::shared_ptr<ActionDataAbstract> createData();
42  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
43 
44  const Vector2s& get_cost_weights() const;
45  void set_cost_weights(const Vector2s& weights);
46 
47  Scalar get_dt() const;
48  void set_dt(const Scalar dt);
49 
55  virtual void print(std::ostream& os) const;
56 
57  protected:
58  using Base::nu_;
59  using Base::state_;
60 
61  private:
62  Vector2s cost_weights_;
63  Scalar dt_;
64 };
65 
66 template <typename _Scalar>
67 struct ActionDataUnicycleTpl : public ActionDataAbstractTpl<_Scalar> {
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 
70  typedef _Scalar Scalar;
73  using Base::cost;
74  using Base::Fu;
75  using Base::Fx;
76  using Base::Lu;
77  using Base::Luu;
78  using Base::Lx;
79  using Base::Lxu;
80  using Base::Lxx;
81  using Base::r;
82  using Base::xnext;
83 
84  template <template <typename Scalar> class Model>
85  explicit ActionDataUnicycleTpl(Model<Scalar>* const model) : Base(model) {
86  Fx.diagonal().array() = Scalar(1.);
87  }
88 };
89 
90 } // namespace crocoddyl
91 
92 /* --- Details -------------------------------------------------------------- */
93 /* --- Details -------------------------------------------------------------- */
94 /* --- Details -------------------------------------------------------------- */
95 #include "crocoddyl/core/actions/unicycle.hxx"
96 
97 #endif // CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
MatrixXs Lxx
Hessian of the cost function.
std::size_t nu_
Control dimension.
VectorXs xnext
evolution state
virtual void print(std::ostream &os) const
Print relevant information of the unicycle model.
VectorXs r
Cost residual.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
MatrixXs Luu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
MatrixXs Lxu
Hessian of the cost function.
MatrixXs Fu
Jacobian of the dynamics.