9 #include "crocoddyl/core/utils/exception.hpp" 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) {
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);
27 template <
typename Scalar>
28 ActionModelLQRTpl<Scalar>::~ActionModelLQRTpl() {}
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()) +
")");
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_) +
")");
43 data->xnext.noalias() = Fx_ * x + Fu_ * u;
45 data->xnext.noalias() = Fx_ * x + Fu_ * u + f0_;
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);
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()) +
")");
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_) +
")");
62 data->Lx.noalias() = lx_ + Lxx_ * x + Lxu_ * u;
63 data->Lu.noalias() = lu_ + Lxu_.transpose() * x + Luu_ * u;
71 template <
typename Scalar>
72 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelLQRTpl<Scalar>::createData() {
73 return boost::make_shared<ActionDataLQRTpl<Scalar> >(
this);
76 template <
typename Scalar>
77 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Fx()
const {
81 template <
typename Scalar>
82 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Fu()
const {
86 template <
typename Scalar>
87 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_f0()
const {
91 template <
typename Scalar>
92 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_lx()
const {
96 template <
typename Scalar>
97 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_lu()
const {
101 template <
typename Scalar>
102 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Lxx()
const {
106 template <
typename Scalar>
107 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Lxu()
const {
111 template <
typename Scalar>
112 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Luu()
const {
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()) +
")");
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_) +
")");
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()) +
")");
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()) +
")");
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_) +
")");
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()) +
")");
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_) +
")");
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_) +
")");
Definition: action-base.hxx:11