1 #ifndef __quadruped_walkgen_quadruped_time_hxx__
2 #define __quadruped_walkgen_quadruped_time_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
10 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(21), 1, 22) {
15 heuristic_weights_.setConstant(
Scalar(1));
17 dt_bound_weight_cmd =
Scalar(100.);
18 dt_weight_cmd =
Scalar(0.);
20 pheuristic_.setZero();
21 gait_double_.setZero();
36 centrifugal_term =
true;
43 rub_max_bool.setZero();
44 dt_ref_.setConstant(
Scalar(0.02));
45 dt_min_.setConstant(
Scalar(0.005));
46 dt_max_.setConstant(
Scalar(0.1));
53 template <
typename Scalar>
56 template <
typename Scalar>
58 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
59 const Eigen::Ref<const typename MathBase::VectorXs>& x,
60 const Eigen::Ref<const typename MathBase::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 " +
64 std::to_string(state_->get_nx()) +
")");
66 if (
static_cast<std::size_t
>(u.size()) != nu_) {
67 throw_pretty(
"Invalid argument: "
68 <<
"u has wrong dimension (it should be " +
69 std::to_string(nu_) +
")");
75 d->xnext.template head<12>() = x.head(12);
76 d->xnext.template segment<8>(12) = x.segment(12, 8);
77 d->xnext.template tail<1>() = u.cwiseAbs();
81 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
83 d->r.template segment<8>(12) =
84 ((heuristic_weights_.cwiseProduct(x.segment(12, 8) - pheuristic_))
89 d->r.template tail<1>() << dt_weight_cmd * (u.cwiseAbs() - dt_ref_);
92 rub_max_ << dt_min_ - u.cwiseAbs(), u.cwiseAbs() - dt_max_;
94 (rub_max_.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
95 rub_max_ = rub_max_.cwiseMax(
Scalar(0.));
97 d->cost =
Scalar(0.5) * d->r.transpose() * d->r +
98 dt_bound_weight_cmd *
Scalar(0.5) * rub_max_.squaredNorm();
103 Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12);
104 cost_[1] =
Scalar(0.5) * d->r.segment(12, 8).transpose() *
106 cost_[2] =
Scalar(0.5) * d->r.tail(1).transpose() *
108 cost_[3] = dt_bound_weight_cmd *
Scalar(0.5) *
109 rub_max_.squaredNorm();
116 template <
typename Scalar>
118 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
119 const Eigen::Ref<const typename MathBase::VectorXs>& x,
120 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
121 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
122 throw_pretty(
"Invalid argument: "
123 <<
"x has wrong dimension (it should be " +
124 std::to_string(state_->get_nx()) +
")");
126 if (
static_cast<std::size_t
>(u.size()) != nu_) {
127 throw_pretty(
"Invalid argument: "
128 <<
"u has wrong dimension (it should be " +
129 std::to_string(nu_) +
")");
136 d->Lx.template head<12>() =
137 (state_weights_.array() * d->r.template head<12>().array()).matrix();
138 d->Lx.template segment<8>(12) =
139 (heuristic_weights_.array() * d->r.template segment<8>(12).array())
142 d->Lu << dt_bound_weight_cmd * std::copysign(1., u(0)) *
143 (-rub_max_[0] + rub_max_[1]);
144 d->Lu += dt_weight_cmd * std::copysign(1., u(0)) * d->r.template tail<1>();
147 d->Lxx.diagonal().head(12) =
148 (state_weights_.array() * state_weights_.array()).matrix();
149 d->Lxx.diagonal().segment(12, 8) =
150 (gait_double_.array() * heuristic_weights_.array() *
151 heuristic_weights_.array())
154 d->Luu.diagonal() << dt_weight_cmd * dt_weight_cmd +
155 dt_bound_weight_cmd * rub_max_bool[0] +
156 dt_bound_weight_cmd * rub_max_bool[1];
160 d->Fx(20, 20) =
Scalar(0.);
161 d->Fu.block(20, 0, 1, 1) << std::copysign(1., u(0));
164 template <
typename Scalar>
165 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
167 return boost::make_shared<ActionDataQuadrupedTimeTpl<Scalar> >(
this);
174 template <
typename Scalar>
175 const typename Eigen::Matrix<Scalar, 12, 1>&
177 return state_weights_;
179 template <
typename Scalar>
181 const typename MathBase::VectorXs& weights) {
182 if (
static_cast<std::size_t
>(weights.size()) != 12) {
183 throw_pretty(
"Invalid argument: "
184 <<
"Weights vector has wrong dimension (it should be 12)");
186 state_weights_ = weights;
189 template <
typename Scalar>
190 const typename Eigen::Matrix<Scalar, 8, 1>&
192 return heuristic_weights_;
194 template <
typename Scalar>
196 const typename MathBase::VectorXs& weights) {
197 if (
static_cast<std::size_t
>(weights.size()) != 8) {
198 throw_pretty(
"Invalid argument: "
199 <<
"Weights vector has wrong dimension (it should be 8)");
201 heuristic_weights_ = weights;
208 template <
typename Scalar>
210 return symmetry_term;
212 template <
typename Scalar>
214 const bool& sym_term) {
216 symmetry_term = sym_term;
219 template <
typename Scalar>
221 return centrifugal_term;
223 template <
typename Scalar>
225 const bool& cent_term) {
227 centrifugal_term = cent_term;
230 template <
typename Scalar>
235 template <
typename Scalar>
242 template <
typename Scalar>
246 template <
typename Scalar>
254 template <
typename Scalar>
258 template <
typename Scalar>
262 template <
typename Scalar>
266 template <
typename Scalar>
273 template <
typename Scalar>
276 return dt_bound_weight_cmd;
278 template <
typename Scalar>
281 dt_bound_weight_cmd = weight_;
287 template <
typename Scalar>
289 return dt_weight_cmd;
291 template <
typename Scalar>
294 dt_weight_cmd = weight_;
300 template <
typename Scalar>
301 const typename Eigen::Matrix<Scalar, 7, 1>&
310 template <
typename Scalar>
312 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
313 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
314 const Eigen::Ref<const typename MathBase::VectorXs>& S) {
315 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
316 throw_pretty(
"Invalid argument: "
317 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
319 if (
static_cast<std::size_t
>(xref.size()) != 12) {
320 throw_pretty(
"Invalid argument: "
321 <<
"Weights vector has wrong dimension (it should be " +
322 std::to_string(state_->get_nx()) +
")");
324 if (
static_cast<std::size_t
>(S.size()) != 4) {
325 throw_pretty(
"Invalid argument: "
326 <<
"S vector has wrong dimension (it should be 4x1)");
331 for (
int i = 0; i < 4; i = i + 1) {
332 gait_double_[2 * i] = S[i];
333 gait_double_[2 * i + 1] = S[i];
334 pheuristic_[2 * i] = l_feet(0, i);
335 pheuristic_[2 * i + 1] = l_feet(1, i);