lqr.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 
11 namespace crocoddyl {
12 
13 template <typename Scalar>
14 ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const std::size_t& nx, const std::size_t& nu, bool drift_free)
15  : Base(boost::make_shared<StateVector>(nx), nu, 0), drift_free_(drift_free) {
16  // TODO(cmastalli): substitute by random (vectors) and random-orthogonal (matrices)
17  Fx_ = MatrixXs::Identity(nx, nx);
18  Fu_ = MatrixXs::Identity(nx, nu);
19  f0_ = VectorXs::Ones(nx);
20  Lxx_ = MatrixXs::Identity(nx, nx);
21  Lxu_ = MatrixXs::Identity(nx, nu);
22  Luu_ = MatrixXs::Identity(nu, nu);
23  lx_ = VectorXs::Ones(nx);
24  lu_ = VectorXs::Ones(nu);
25 }
26 
27 template <typename Scalar>
28 ActionModelLQRTpl<Scalar>::~ActionModelLQRTpl() {}
29 
30 template <typename Scalar>
31 void ActionModelLQRTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
32  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
33  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
34  throw_pretty("Invalid argument: "
35  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
36  }
37  if (static_cast<std::size_t>(u.size()) != nu_) {
38  throw_pretty("Invalid argument: "
39  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
40  }
41 
42  if (drift_free_) {
43  data->xnext.noalias() = Fx_ * x + Fu_ * u;
44  } else {
45  data->xnext.noalias() = Fx_ * x + Fu_ * u + f0_;
46  }
47  data->cost = 0.5 * x.dot(Lxx_ * x) + 0.5 * u.dot(Luu_ * u) + x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
48 }
49 
50 template <typename Scalar>
51 void ActionModelLQRTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
52  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
53  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
54  throw_pretty("Invalid argument: "
55  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
56  }
57  if (static_cast<std::size_t>(u.size()) != nu_) {
58  throw_pretty("Invalid argument: "
59  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
60  }
61 
62  data->Lx.noalias() = lx_ + Lxx_ * x + Lxu_ * u;
63  data->Lu.noalias() = lu_ + Lxu_.transpose() * x + Luu_ * u;
64  data->Fx = Fx_;
65  data->Fu = Fu_;
66  data->Lxx = Lxx_;
67  data->Lxu = Lxu_;
68  data->Luu = Luu_;
69 }
70 
71 template <typename Scalar>
72 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelLQRTpl<Scalar>::createData() {
73  return boost::make_shared<ActionDataLQRTpl<Scalar> >(this);
74 }
75 
76 template <typename Scalar>
77 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Fx() const {
78  return Fx_;
79 }
80 
81 template <typename Scalar>
82 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Fu() const {
83  return Fu_;
84 }
85 
86 template <typename Scalar>
87 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_f0() const {
88  return f0_;
89 }
90 
91 template <typename Scalar>
92 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_lx() const {
93  return lx_;
94 }
95 
96 template <typename Scalar>
97 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_lu() const {
98  return lu_;
99 }
100 
101 template <typename Scalar>
102 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Lxx() const {
103  return Lxx_;
104 }
105 
106 template <typename Scalar>
107 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Lxu() const {
108  return Lxu_;
109 }
110 
111 template <typename Scalar>
112 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Luu() const {
113  return Luu_;
114 }
115 
116 template <typename Scalar>
117 void ActionModelLQRTpl<Scalar>::set_Fx(const MatrixXs& Fx) {
118  if (static_cast<std::size_t>(Fx.rows()) != state_->get_nx() ||
119  static_cast<std::size_t>(Fx.cols()) != state_->get_nx()) {
120  throw_pretty("Invalid argument: "
121  << "Fx has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
122  std::to_string(state_->get_nx()) + ")");
123  }
124  Fx_ = Fx;
125 }
126 
127 template <typename Scalar>
128 void ActionModelLQRTpl<Scalar>::set_Fu(const MatrixXs& Fu) {
129  if (static_cast<std::size_t>(Fu.rows()) != state_->get_nx() || static_cast<std::size_t>(Fu.cols()) != nu_) {
130  throw_pretty("Invalid argument: "
131  << "Fu has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
132  std::to_string(nu_) + ")");
133  }
134  Fu_ = Fu;
135 }
136 
137 template <typename Scalar>
138 void ActionModelLQRTpl<Scalar>::set_f0(const VectorXs& f0) {
139  if (static_cast<std::size_t>(f0.size()) != state_->get_nx()) {
140  throw_pretty("Invalid argument: "
141  << "f0 has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
142  }
143  f0_ = f0;
144 }
145 
146 template <typename Scalar>
147 void ActionModelLQRTpl<Scalar>::set_lx(const VectorXs& lx) {
148  if (static_cast<std::size_t>(lx.size()) != state_->get_nx()) {
149  throw_pretty("Invalid argument: "
150  << "lx has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
151  }
152  lx_ = lx;
153 }
154 
155 template <typename Scalar>
156 void ActionModelLQRTpl<Scalar>::set_lu(const VectorXs& lu) {
157  if (static_cast<std::size_t>(lu.size()) != nu_) {
158  throw_pretty("Invalid argument: "
159  << "lu has wrong dimension (it should be " + std::to_string(nu_) + ")");
160  }
161  lu_ = lu;
162 }
163 
164 template <typename Scalar>
165 void ActionModelLQRTpl<Scalar>::set_Lxx(const MatrixXs& Lxx) {
166  if (static_cast<std::size_t>(Lxx.rows()) != state_->get_nx() ||
167  static_cast<std::size_t>(Lxx.cols()) != state_->get_nx()) {
168  throw_pretty("Invalid argument: "
169  << "Lxx has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
170  std::to_string(state_->get_nx()) + ")");
171  }
172  Lxx_ = Lxx;
173 }
174 
175 template <typename Scalar>
176 void ActionModelLQRTpl<Scalar>::set_Lxu(const MatrixXs& Lxu) {
177  if (static_cast<std::size_t>(Lxu.rows()) != state_->get_nx() || static_cast<std::size_t>(Lxu.cols()) != nu_) {
178  throw_pretty("Invalid argument: "
179  << "Lxu has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
180  std::to_string(nu_) + ")");
181  }
182  Lxu_ = Lxu;
183 }
184 
185 template <typename Scalar>
186 void ActionModelLQRTpl<Scalar>::set_Luu(const MatrixXs& Luu) {
187  if (static_cast<std::size_t>(Luu.rows()) != nu_ || static_cast<std::size_t>(Luu.cols()) != nu_) {
188  throw_pretty("Invalid argument: "
189  << "Fq has wrong dimension (it should be " + std::to_string(nu_) + "," + std::to_string(nu_) + ")");
190  }
191  Luu_ = Luu;
192 }
193 
194 } // namespace crocoddyl
Definition: action-base.hxx:11