Crocoddyl
 
Loading...
Searching...
No Matches
unicycle.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
11#define CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
12
13#include "crocoddyl/core/action-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15#include "crocoddyl/core/states/euclidean.hpp"
16
17namespace crocoddyl {
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 CROCODDYL_DERIVED_CAST(ActionModelBase, ActionModelUnicycleTpl)
23
24 typedef _Scalar Scalar;
29 typedef typename MathBase::VectorXs VectorXs;
30 typedef typename MathBase::Vector2s Vector2s;
31
33 virtual ~ActionModelUnicycleTpl() = default;
34
35 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
36 const Eigen::Ref<const VectorXs>& x,
37 const Eigen::Ref<const VectorXs>& u) override;
38 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
39 const Eigen::Ref<const VectorXs>& x) override;
40 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
41 const Eigen::Ref<const VectorXs>& x,
42 const Eigen::Ref<const VectorXs>& u) override;
43 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
44 const Eigen::Ref<const VectorXs>& x) override;
45 virtual std::shared_ptr<ActionDataAbstract> createData() override;
46
56 template <typename NewScalar>
58
59 virtual bool checkData(
60 const std::shared_ptr<ActionDataAbstract>& data) override;
61
62 const Vector2s& get_cost_weights() const;
63 void set_cost_weights(const Vector2s& weights);
64
65 Scalar get_dt() const;
66 void set_dt(const Scalar dt);
67
73 virtual void print(std::ostream& os) const override;
74
75 protected:
76 using Base::nu_;
77 using Base::state_;
78
79 private:
80 Vector2s cost_weights_;
81 Scalar dt_;
82};
83
84template <typename _Scalar>
86 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87
88 typedef _Scalar Scalar;
91 using Base::cost;
92 using Base::Fu;
93 using Base::Fx;
94 using Base::Lu;
95 using Base::Luu;
96 using Base::Lx;
97 using Base::Lxu;
98 using Base::Lxx;
99 using Base::r;
100 using Base::xnext;
101
102 template <template <typename Scalar> class Model>
103 explicit ActionDataUnicycleTpl(Model<Scalar>* const model) : Base(model) {
104 Fx.diagonal().array() = Scalar(1.);
105 }
106 virtual ~ActionDataUnicycleTpl() = default;
107};
108
109} // namespace crocoddyl
110
111/* --- Details -------------------------------------------------------------- */
112/* --- Details -------------------------------------------------------------- */
113/* --- Details -------------------------------------------------------------- */
114#include "crocoddyl/core/actions/unicycle.hxx"
115
116CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActionModelUnicycleTpl)
117CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActionDataUnicycleTpl)
118
119#endif // CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
Abstract class for action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the next state and cost value.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the action data.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the cost functions with respect to the state only.
virtual bool checkData(const std::shared_ptr< ActionDataAbstract > &data) override
Checks that a specific data belongs to this model.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the total cost value for nodes that depends only on the state.
ActionModelUnicycleTpl< NewScalar > cast() const
Cast the unicycle model to a different scalar type.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics and cost functions.
virtual void print(std::ostream &os) const override
Print relevant information of the unicycle model.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
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 .
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .