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_DIFF_LQR_HPP_ |
10 |
|
|
#define CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_ |
11 |
|
|
|
12 |
|
|
#include <stdexcept> |
13 |
|
|
|
14 |
|
|
#include "crocoddyl/core/diff-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 DifferentialActionModelLQRTpl |
22 |
|
|
: public DifferentialActionModelAbstractTpl<_Scalar> { |
23 |
|
|
public: |
24 |
|
|
typedef _Scalar Scalar; |
25 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
26 |
|
|
typedef DifferentialActionModelAbstractTpl<Scalar> Base; |
27 |
|
|
typedef DifferentialActionDataLQRTpl<Scalar> Data; |
28 |
|
|
typedef StateVectorTpl<Scalar> StateVector; |
29 |
|
|
typedef DifferentialActionDataAbstractTpl<Scalar> |
30 |
|
|
DifferentialActionDataAbstract; |
31 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
32 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
33 |
|
|
|
34 |
|
|
DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu, |
35 |
|
|
const bool drift_free = true); |
36 |
|
|
virtual ~DifferentialActionModelLQRTpl(); |
37 |
|
|
|
38 |
|
|
virtual void calc( |
39 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
40 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u); |
41 |
|
|
virtual void calc( |
42 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
43 |
|
|
const Eigen::Ref<const VectorXs>& x); |
44 |
|
|
virtual void calcDiff( |
45 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
46 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u); |
47 |
|
|
virtual void calcDiff( |
48 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
49 |
|
|
const Eigen::Ref<const VectorXs>& x); |
50 |
|
|
virtual boost::shared_ptr<DifferentialActionDataAbstract> createData(); |
51 |
|
|
virtual bool checkData( |
52 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>& data); |
53 |
|
|
|
54 |
|
|
const MatrixXs& get_Fq() const; |
55 |
|
|
const MatrixXs& get_Fv() const; |
56 |
|
|
const MatrixXs& get_Fu() const; |
57 |
|
|
const VectorXs& get_f0() const; |
58 |
|
|
const VectorXs& get_lx() const; |
59 |
|
|
const VectorXs& get_lu() const; |
60 |
|
|
const MatrixXs& get_Lxx() const; |
61 |
|
|
const MatrixXs& get_Lxu() const; |
62 |
|
|
const MatrixXs& get_Luu() const; |
63 |
|
|
|
64 |
|
|
void set_Fq(const MatrixXs& Fq); |
65 |
|
|
void set_Fv(const MatrixXs& Fv); |
66 |
|
|
void set_Fu(const MatrixXs& Fu); |
67 |
|
|
void set_f0(const VectorXs& f0); |
68 |
|
|
void set_lx(const VectorXs& lx); |
69 |
|
|
void set_lu(const VectorXs& lu); |
70 |
|
|
void set_Lxx(const MatrixXs& Lxx); |
71 |
|
|
void set_Lxu(const MatrixXs& Lxu); |
72 |
|
|
void set_Luu(const MatrixXs& Luu); |
73 |
|
|
|
74 |
|
|
/** |
75 |
|
|
* @brief Print relevant information of the LQR model |
76 |
|
|
* |
77 |
|
|
* @param[out] os Output stream object |
78 |
|
|
*/ |
79 |
|
|
virtual void print(std::ostream& os) const; |
80 |
|
|
|
81 |
|
|
protected: |
82 |
|
|
using Base::nu_; //!< Control dimension |
83 |
|
|
using Base::state_; //!< Model of the state |
84 |
|
|
|
85 |
|
|
private: |
86 |
|
|
bool drift_free_; |
87 |
|
|
MatrixXs Fq_; |
88 |
|
|
MatrixXs Fv_; |
89 |
|
|
MatrixXs Fu_; |
90 |
|
|
VectorXs f0_; |
91 |
|
|
MatrixXs Lxx_; |
92 |
|
|
MatrixXs Lxu_; |
93 |
|
|
MatrixXs Luu_; |
94 |
|
|
VectorXs lx_; |
95 |
|
|
VectorXs lu_; |
96 |
|
|
}; |
97 |
|
|
|
98 |
|
|
template <typename _Scalar> |
99 |
|
|
struct DifferentialActionDataLQRTpl |
100 |
|
|
: public DifferentialActionDataAbstractTpl<_Scalar> { |
101 |
|
|
typedef _Scalar Scalar; |
102 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
103 |
|
|
typedef DifferentialActionDataAbstractTpl<Scalar> Base; |
104 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
105 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
106 |
|
|
|
107 |
|
|
template <template <typename Scalar> class Model> |
108 |
|
13554 |
explicit DifferentialActionDataLQRTpl(Model<Scalar>* const model) |
109 |
|
13554 |
: Base(model) { |
110 |
|
|
// Setting the linear model and quadratic cost here because they are |
111 |
|
|
// constant |
112 |
✓✗✓✗
|
13554 |
Fx.leftCols(model->get_state()->get_nq()) = model->get_Fq(); |
113 |
✓✗✓✗
|
13554 |
Fx.rightCols(model->get_state()->get_nv()) = model->get_Fv(); |
114 |
✓✗ |
13554 |
Fu = model->get_Fu(); |
115 |
✓✗ |
13554 |
Lxx = model->get_Lxx(); |
116 |
✓✗ |
13554 |
Luu = model->get_Luu(); |
117 |
✓✗ |
13554 |
Lxu = model->get_Lxu(); |
118 |
|
13554 |
} |
119 |
|
|
|
120 |
|
|
using Base::cost; |
121 |
|
|
using Base::Fu; |
122 |
|
|
using Base::Fx; |
123 |
|
|
using Base::Lu; |
124 |
|
|
using Base::Luu; |
125 |
|
|
using Base::Lx; |
126 |
|
|
using Base::Lxu; |
127 |
|
|
using Base::Lxx; |
128 |
|
|
using Base::r; |
129 |
|
|
using Base::xout; |
130 |
|
|
}; |
131 |
|
|
|
132 |
|
|
} // namespace crocoddyl |
133 |
|
|
|
134 |
|
|
/* --- Details -------------------------------------------------------------- */ |
135 |
|
|
/* --- Details -------------------------------------------------------------- */ |
136 |
|
|
/* --- Details -------------------------------------------------------------- */ |
137 |
|
|
#include "crocoddyl/core/actions/diff-lqr.hxx" |
138 |
|
|
#endif // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_ |