diff-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 #include "crocoddyl/core/actions/diff-lqr.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl(const std::size_t& nq, const std::size_t& nu,
16  bool drift_free)
17  : Base(boost::make_shared<StateVector>(2 * nq), nu), drift_free_(drift_free) {
18  // TODO(cmastalli): substitute by random (vectors) and random-orthogonal (matrices)
19  Fq_ = MatrixXs::Identity(state_->get_nq(), state_->get_nq());
20  Fv_ = MatrixXs::Identity(state_->get_nv(), state_->get_nv());
21  Fu_ = MatrixXs::Identity(state_->get_nq(), nu_);
22  f0_ = VectorXs::Ones(state_->get_nv());
23  Lxx_ = MatrixXs::Identity(state_->get_nx(), state_->get_nx());
24  Lxu_ = MatrixXs::Identity(state_->get_nx(), nu_);
25  Luu_ = MatrixXs::Identity(nu_, nu_);
26  lx_ = VectorXs::Ones(state_->get_nx());
27  lu_ = VectorXs::Ones(nu_);
28 }
29 
30 template <typename Scalar>
31 DifferentialActionModelLQRTpl<Scalar>::~DifferentialActionModelLQRTpl() {}
32 
33 template <typename Scalar>
34 void DifferentialActionModelLQRTpl<Scalar>::calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
35  const Eigen::Ref<const VectorXs>& x,
36  const Eigen::Ref<const VectorXs>& u) {
37  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
38  throw_pretty("Invalid argument: "
39  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
40  }
41  if (static_cast<std::size_t>(u.size()) != nu_) {
42  throw_pretty("Invalid argument: "
43  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
44  }
45 
46  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
47  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
48 
49  if (drift_free_) {
50  data->xout = Fq_ * q + Fv_ * v + Fu_ * u;
51  } else {
52  data->xout = Fq_ * q + Fv_ * v + Fu_ * u + f0_;
53  }
54  data->cost = 0.5 * x.dot(Lxx_ * x) + 0.5 * u.dot(Luu_ * u) + x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
55 }
56 
57 template <typename Scalar>
58 void DifferentialActionModelLQRTpl<Scalar>::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
59  const Eigen::Ref<const VectorXs>& x,
60  const Eigen::Ref<const VectorXs>& u) {
61  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
62  throw_pretty("Invalid argument: "
63  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
64  }
65  if (static_cast<std::size_t>(u.size()) != nu_) {
66  throw_pretty("Invalid argument: "
67  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
68  }
69 
70  data->Lx = lx_ + Lxx_ * x + Lxu_ * u;
71  data->Lu = lu_ + Lxu_.transpose() * x + Luu_ * u;
72  data->Fx.leftCols(state_->get_nq()) = Fq_;
73  data->Fx.rightCols(state_->get_nv()) = Fv_;
74  data->Fu = Fu_;
75  data->Lxx = Lxx_;
76  data->Lxu = Lxu_;
77  data->Luu = Luu_;
78 }
79 
80 template <typename Scalar>
81 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > DifferentialActionModelLQRTpl<Scalar>::createData() {
82  return boost::make_shared<DifferentialActionDataLQR>(this);
83 }
84 
85 template <typename Scalar>
86 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fq() const {
87  return Fq_;
88 }
89 
90 template <typename Scalar>
91 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fv() const {
92  return Fv_;
93 }
94 
95 template <typename Scalar>
96 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fu() const {
97  return Fu_;
98 }
99 
100 template <typename Scalar>
101 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_f0() const {
102  return f0_;
103 }
104 
105 template <typename Scalar>
106 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_lx() const {
107  return lx_;
108 }
109 
110 template <typename Scalar>
111 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_lu() const {
112  return lu_;
113 }
114 
115 template <typename Scalar>
116 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Lxx() const {
117  return Lxx_;
118 }
119 
120 template <typename Scalar>
121 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Lxu() const {
122  return Lxu_;
123 }
124 
125 template <typename Scalar>
126 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Luu() const {
127  return Luu_;
128 }
129 
130 template <typename Scalar>
131 void DifferentialActionModelLQRTpl<Scalar>::set_Fq(const MatrixXs& Fq) {
132  if (static_cast<std::size_t>(Fq.rows()) != state_->get_nq() ||
133  static_cast<std::size_t>(Fq.cols()) != state_->get_nq()) {
134  throw_pretty("Invalid argument: "
135  << "Fq has wrong dimension (it should be " + std::to_string(state_->get_nq()) + "," +
136  std::to_string(state_->get_nq()) + ")");
137  }
138  Fq_ = Fq;
139 }
140 
141 template <typename Scalar>
142 void DifferentialActionModelLQRTpl<Scalar>::set_Fv(const MatrixXs& Fv) {
143  if (static_cast<std::size_t>(Fv.rows()) != state_->get_nv() ||
144  static_cast<std::size_t>(Fv.cols()) != state_->get_nv()) {
145  throw_pretty("Invalid argument: "
146  << "Fv has wrong dimension (it should be " + std::to_string(state_->get_nv()) + "," +
147  std::to_string(state_->get_nv()) + ")");
148  }
149  Fv_ = Fv;
150 }
151 
152 template <typename Scalar>
153 void DifferentialActionModelLQRTpl<Scalar>::set_Fu(const MatrixXs& Fu) {
154  if (static_cast<std::size_t>(Fu.rows()) != state_->get_nq() || static_cast<std::size_t>(Fu.cols()) != nu_) {
155  throw_pretty("Invalid argument: "
156  << "Fu has wrong dimension (it should be " + std::to_string(state_->get_nq()) + "," +
157  std::to_string(nu_) + ")");
158  }
159  Fu_ = Fu;
160 }
161 
162 template <typename Scalar>
163 void DifferentialActionModelLQRTpl<Scalar>::set_f0(const VectorXs& f0) {
164  if (static_cast<std::size_t>(f0.size()) != state_->get_nv()) {
165  throw_pretty("Invalid argument: "
166  << "f0 has wrong dimension (it should be " + std::to_string(state_->get_nv()) + ")");
167  }
168  f0_ = f0;
169 }
170 
171 template <typename Scalar>
172 void DifferentialActionModelLQRTpl<Scalar>::set_lx(const VectorXs& lx) {
173  if (static_cast<std::size_t>(lx.size()) != state_->get_nx()) {
174  throw_pretty("Invalid argument: "
175  << "lx has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
176  }
177  lx_ = lx;
178 }
179 
180 template <typename Scalar>
181 void DifferentialActionModelLQRTpl<Scalar>::set_lu(const VectorXs& lu) {
182  if (static_cast<std::size_t>(lu.size()) != nu_) {
183  throw_pretty("Invalid argument: "
184  << "lu has wrong dimension (it should be " + std::to_string(nu_) + ")");
185  }
186  lu_ = lu;
187 }
188 
189 template <typename Scalar>
190 void DifferentialActionModelLQRTpl<Scalar>::set_Lxx(const MatrixXs& Lxx) {
191  if (static_cast<std::size_t>(Lxx.rows()) != state_->get_nx() ||
192  static_cast<std::size_t>(Lxx.cols()) != state_->get_nx()) {
193  throw_pretty("Invalid argument: "
194  << "Lxx has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
195  std::to_string(state_->get_nx()) + ")");
196  }
197  Lxx_ = Lxx;
198 }
199 
200 template <typename Scalar>
201 void DifferentialActionModelLQRTpl<Scalar>::set_Lxu(const MatrixXs& Lxu) {
202  if (static_cast<std::size_t>(Lxu.rows()) != state_->get_nx() || static_cast<std::size_t>(Lxu.cols()) != nu_) {
203  throw_pretty("Invalid argument: "
204  << "Lxu has wrong dimension (it should be " + std::to_string(state_->get_nx()) + "," +
205  std::to_string(nu_) + ")");
206  }
207  Lxu_ = Lxu;
208 }
209 
210 template <typename Scalar>
211 void DifferentialActionModelLQRTpl<Scalar>::set_Luu(const MatrixXs& Luu) {
212  if (static_cast<std::size_t>(Luu.rows()) != nu_ || static_cast<std::size_t>(Luu.cols()) != nu_) {
213  throw_pretty("Invalid argument: "
214  << "Fq has wrong dimension (it should be " + std::to_string(nu_) + "," + std::to_string(nu_) + ")");
215  }
216  Luu_ = Luu;
217 }
218 
219 } // namespace crocoddyl
Definition: action-base.hxx:11