1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// 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. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#ifndef CROCODDYL_CORE_ACTIONS_LQR_HPP_ |
10 |
|
|
#define CROCODDYL_CORE_ACTIONS_LQR_HPP_ |
11 |
|
|
|
12 |
|
|
#include <stdexcept> |
13 |
|
|
|
14 |
|
|
#include "crocoddyl/core/action-base.hpp" |
15 |
|
|
#include "crocoddyl/core/fwd.hpp" |
16 |
|
|
#include "crocoddyl/core/states/euclidean.hpp" |
17 |
|
|
|
18 |
|
|
namespace crocoddyl { |
19 |
|
|
|
20 |
|
|
template <typename _Scalar> |
21 |
|
|
class ActionModelLQRTpl : public ActionModelAbstractTpl<_Scalar> { |
22 |
|
|
public: |
23 |
|
|
typedef _Scalar Scalar; |
24 |
|
|
typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract; |
25 |
|
|
typedef ActionModelAbstractTpl<Scalar> Base; |
26 |
|
|
typedef ActionDataLQRTpl<Scalar> Data; |
27 |
|
|
typedef StateVectorTpl<Scalar> StateVector; |
28 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
29 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
30 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
31 |
|
|
|
32 |
|
|
ActionModelLQRTpl(const std::size_t nx, const std::size_t nu, |
33 |
|
|
const bool drift_free = true); |
34 |
|
|
virtual ~ActionModelLQRTpl(); |
35 |
|
|
|
36 |
|
|
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, |
37 |
|
|
const Eigen::Ref<const VectorXs>& x, |
38 |
|
|
const Eigen::Ref<const VectorXs>& u); |
39 |
|
|
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, |
40 |
|
|
const Eigen::Ref<const VectorXs>& x); |
41 |
|
|
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, |
42 |
|
|
const Eigen::Ref<const VectorXs>& x, |
43 |
|
|
const Eigen::Ref<const VectorXs>& u); |
44 |
|
|
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, |
45 |
|
|
const Eigen::Ref<const VectorXs>& x); |
46 |
|
|
virtual boost::shared_ptr<ActionDataAbstract> createData(); |
47 |
|
|
virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data); |
48 |
|
|
|
49 |
|
|
const MatrixXs& get_Fx() const; |
50 |
|
|
const MatrixXs& get_Fu() const; |
51 |
|
|
const VectorXs& get_f0() const; |
52 |
|
|
const VectorXs& get_lx() const; |
53 |
|
|
const VectorXs& get_lu() const; |
54 |
|
|
const MatrixXs& get_Lxx() const; |
55 |
|
|
const MatrixXs& get_Lxu() const; |
56 |
|
|
const MatrixXs& get_Luu() const; |
57 |
|
|
|
58 |
|
|
void set_Fx(const MatrixXs& Fx); |
59 |
|
|
void set_Fu(const MatrixXs& Fu); |
60 |
|
|
void set_f0(const VectorXs& f0); |
61 |
|
|
void set_lx(const VectorXs& lx); |
62 |
|
|
void set_lu(const VectorXs& lu); |
63 |
|
|
void set_Lxx(const MatrixXs& Lxx); |
64 |
|
|
void set_Lxu(const MatrixXs& Lxu); |
65 |
|
|
void set_Luu(const MatrixXs& Luu); |
66 |
|
|
|
67 |
|
|
/** |
68 |
|
|
* @brief Print relevant information of the LQR model |
69 |
|
|
* |
70 |
|
|
* @param[out] os Output stream object |
71 |
|
|
*/ |
72 |
|
|
virtual void print(std::ostream& os) const; |
73 |
|
|
|
74 |
|
|
protected: |
75 |
|
|
using Base::nu_; //!< Control dimension |
76 |
|
|
using Base::state_; //!< Model of the state |
77 |
|
|
|
78 |
|
|
private: |
79 |
|
|
bool drift_free_; |
80 |
|
|
MatrixXs Fx_; |
81 |
|
|
MatrixXs Fu_; |
82 |
|
|
VectorXs f0_; |
83 |
|
|
MatrixXs Lxx_; |
84 |
|
|
MatrixXs Lxu_; |
85 |
|
|
MatrixXs Luu_; |
86 |
|
|
VectorXs lx_; |
87 |
|
|
VectorXs lu_; |
88 |
|
|
}; |
89 |
|
|
|
90 |
|
|
template <typename _Scalar> |
91 |
|
|
struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> { |
92 |
|
|
typedef _Scalar Scalar; |
93 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
94 |
|
|
typedef ActionDataAbstractTpl<Scalar> Base; |
95 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
96 |
|
|
|
97 |
|
|
template <template <typename Scalar> class Model> |
98 |
|
823 |
explicit ActionDataLQRTpl(Model<Scalar>* const model) |
99 |
|
|
: Base(model), |
100 |
|
823 |
Luu_u_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_nu()))), |
101 |
|
|
Lxx_x_tmp(VectorXs::Zero( |
102 |
✓✗✓✗
✓✗✓✗
|
823 |
static_cast<Eigen::Index>(model->get_state()->get_ndx()))) { |
103 |
|
|
// Setting the linear model and quadratic cost here because they are |
104 |
|
|
// constant |
105 |
✓✗ |
823 |
Fx = model->get_Fx(); |
106 |
✓✗ |
823 |
Fu = model->get_Fu(); |
107 |
✓✗ |
823 |
Lxx = model->get_Lxx(); |
108 |
✓✗ |
823 |
Luu = model->get_Luu(); |
109 |
✓✗ |
823 |
Lxu = model->get_Lxu(); |
110 |
|
823 |
} |
111 |
|
|
|
112 |
|
|
using Base::cost; |
113 |
|
|
using Base::Fu; |
114 |
|
|
using Base::Fx; |
115 |
|
|
using Base::Lu; |
116 |
|
|
using Base::Luu; |
117 |
|
|
using Base::Lx; |
118 |
|
|
using Base::Lxu; |
119 |
|
|
using Base::Lxx; |
120 |
|
|
using Base::r; |
121 |
|
|
using Base::xnext; |
122 |
|
|
VectorXs Luu_u_tmp; // Temporary variable for storing Hessian-vector product |
123 |
|
|
// (size: nu) |
124 |
|
|
VectorXs Lxx_x_tmp; // Temporary variable for storing Hessian-vector product |
125 |
|
|
// (size: nx) |
126 |
|
|
}; |
127 |
|
|
|
128 |
|
|
} // namespace crocoddyl |
129 |
|
|
|
130 |
|
|
/* --- Details -------------------------------------------------------------- */ |
131 |
|
|
/* --- Details -------------------------------------------------------------- */ |
132 |
|
|
/* --- Details -------------------------------------------------------------- */ |
133 |
|
|
#include "crocoddyl/core/actions/lqr.hxx" |
134 |
|
|
|
135 |
|
|
#endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_ |