9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/core/actions/diff-lqr.hpp" 14 template <
typename Scalar>
15 DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl(
const std::size_t& nq,
const std::size_t& nu,
17 : Base(boost::make_shared<StateVector>(2 * nq), nu), drift_free_(drift_free) {
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_);
30 template <
typename Scalar>
31 DifferentialActionModelLQRTpl<Scalar>::~DifferentialActionModelLQRTpl() {}
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()) +
")");
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_) +
")");
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());
50 data->xout = Fq_ * q + Fv_ * v + Fu_ * u;
52 data->xout = Fq_ * q + Fv_ * v + Fu_ * u + f0_;
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);
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()) +
")");
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_) +
")");
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_;
80 template <
typename Scalar>
81 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > DifferentialActionModelLQRTpl<Scalar>::createData() {
82 return boost::make_shared<DifferentialActionDataLQR>(
this);
85 template <
typename Scalar>
86 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fq()
const {
90 template <
typename Scalar>
91 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fv()
const {
95 template <
typename Scalar>
96 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Fu()
const {
100 template <
typename Scalar>
101 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_f0()
const {
105 template <
typename Scalar>
106 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_lx()
const {
110 template <
typename Scalar>
111 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelLQRTpl<Scalar>::get_lu()
const {
115 template <
typename Scalar>
116 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Lxx()
const {
120 template <
typename Scalar>
121 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Lxu()
const {
125 template <
typename Scalar>
126 const typename MathBaseTpl<Scalar>::MatrixXs& DifferentialActionModelLQRTpl<Scalar>::get_Luu()
const {
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()) +
")");
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()) +
")");
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_) +
")");
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()) +
")");
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()) +
")");
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_) +
")");
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()) +
")");
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_) +
")");
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_) +
")");
Definition: action-base.hxx:11