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);
ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:8
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:195
~ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:54
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_time.hxx:166
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_time.hxx:236
const Scalar & get_T_gait() const
Definition: quadruped_time.hxx:231
const Scalar & get_dt_max() const
Definition: quadruped_time.hxx:263
const Scalar & get_dt_weight_cmd() const
Definition: quadruped_time.hxx:288
const Scalar & get_dt_bound_weight_cmd() const
Definition: quadruped_time.hxx:274
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_time.hxx:57
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_time.hxx:311
void set_dt_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:292
void set_dt_max(const Scalar &dt)
Definition: quadruped_time.hxx:267
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_time.hxx:213
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_time.hxx:224
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_time.hxx:191
const Scalar & get_dt_min() const
Definition: quadruped_time.hxx:255
void set_dt_ref(const Scalar &dt)
Definition: quadruped_time.hxx:247
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_time.hxx:302
_Scalar Scalar
Definition: quadruped_time.hpp:16
const bool & get_symmetry_term() const
Definition: quadruped_time.hxx:209
const bool & get_centrifugal_term() const
Definition: quadruped_time.hxx:220
const Scalar & get_dt_ref() const
Definition: quadruped_time.hxx:243
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_time.hxx:117
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:180
void set_dt_min(const Scalar &dt)
Definition: quadruped_time.hxx:259
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_time.hxx:176
void set_dt_bound_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:279
Definition: quadruped.hpp:11
Definition: quadruped_time.hpp:119