crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
unicycle.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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 calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
38  const Eigen::Ref<const VectorXs>& u);
39  virtual boost::shared_ptr<ActionDataAbstract> createData();
40  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
41 
42  const Vector2s& get_cost_weights() const;
43  void set_cost_weights(const Vector2s& weights);
44 
45  Scalar get_dt() const;
46  void set_dt(const Scalar dt);
47 
48  protected:
50  using Base::nr_;
51  using Base::nu_;
52  using Base::state_;
53  using Base::u_lb_;
54  using Base::u_ub_;
55  using Base::unone_;
56 
57  private:
58  Vector2s cost_weights_;
59  Scalar dt_;
60 };
61 
62 template <typename _Scalar>
63 struct ActionDataUnicycleTpl : public ActionDataAbstractTpl<_Scalar> {
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 
66  typedef _Scalar Scalar;
69  using Base::cost;
70  using Base::Fu;
71  using Base::Fx;
72  using Base::Lu;
73  using Base::Luu;
74  using Base::Lx;
75  using Base::Lxu;
76  using Base::Lxx;
77  using Base::r;
78  using Base::xnext;
79 
80  template <template <typename Scalar> class Model>
81  explicit ActionDataUnicycleTpl(Model<Scalar>* const model) : Base(model) {
82  Fx.diagonal().array() = Scalar(1.);
83  }
84 };
85 
86 } // namespace crocoddyl
87 
88 /* --- Details -------------------------------------------------------------- */
89 /* --- Details -------------------------------------------------------------- */
90 /* --- Details -------------------------------------------------------------- */
91 #include "crocoddyl/core/actions/unicycle.hxx"
92 
93 #endif // CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
crocoddyl::ActionDataAbstractTpl
Definition: action-base.hpp:224
crocoddyl::ActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:215
crocoddyl::ActionModelUnicycleTpl::calc
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.
crocoddyl::ActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: action-base.hpp:264
crocoddyl::ActionModelUnicycleTpl
Definition: unicycle.hpp:20
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: action-base.hpp:209
crocoddyl::ActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: action-base.hpp:261
crocoddyl::ActionDataAbstractTpl::xnext
VectorXs xnext
evolution state
Definition: action-base.hpp:257
crocoddyl::ActionModelAbstractTpl
Abstract class for action model.
Definition: action-base.hpp:60
crocoddyl::ActionModelUnicycleTpl::calcDiff
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.
crocoddyl::ActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: action-base.hpp:256
crocoddyl::ActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: action-base.hpp:260
crocoddyl::ActionModelUnicycleTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:211
crocoddyl::ActionDataUnicycleTpl
Definition: unicycle.hpp:63
crocoddyl::ActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: action-base.hpp:212
crocoddyl::ActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: action-base.hpp:258
crocoddyl::ActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:213
crocoddyl::ActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:214
crocoddyl::ActionModelUnicycleTpl::checkData
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
crocoddyl::ActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: action-base.hpp:262
crocoddyl::ActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: action-base.hpp:263
crocoddyl::ActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: action-base.hpp:259
crocoddyl::ActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:210
crocoddyl::ActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: action-base.hpp:265