1#ifndef __quadruped_walkgen_quadruped_time_hxx__
2#define __quadruped_walkgen_quadruped_time_hxx__
4#include "crocoddyl/core/utils/exception.hpp"
7template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
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));
53template <
typename Scalar>
56template <
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()) {
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_) {
68 <<
"u has wrong dimension (it should be " +
69 std::to_string(
nu_) +
")");
77 d->xnext.template
tail<1>() =
u.cwiseAbs();
81 d->r.template
head<12>() = state_weights_.cwiseProduct(
x.head(12) - xref_);
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_;
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();
116template <
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()) {
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_) {
128 <<
"u has wrong dimension (it should be " +
129 std::to_string(
nu_) +
")");
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];
161 d->Fu.block(20, 0, 1, 1) << std::copysign(1.,
u(0));
164template <
typename Scalar>
165boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
167 return boost::make_shared<ActionDataQuadrupedTimeTpl<Scalar> >(
this);
174template <
typename Scalar>
175const typename Eigen::Matrix<Scalar, 12, 1>&
177 return state_weights_;
179template <
typename Scalar>
181 const typename MathBase::VectorXs&
weights) {
182 if (
static_cast<std::size_t
>(
weights.size()) != 12) {
184 <<
"Weights vector has wrong dimension (it should be 12)");
189template <
typename Scalar>
190const typename Eigen::Matrix<Scalar, 8, 1>&
192 return heuristic_weights_;
194template <
typename Scalar>
196 const typename MathBase::VectorXs&
weights) {
197 if (
static_cast<std::size_t
>(
weights.size()) != 8) {
199 <<
"Weights vector has wrong dimension (it should be 8)");
208template <
typename Scalar>
210 return symmetry_term;
212template <
typename Scalar>
219template <
typename Scalar>
221 return centrifugal_term;
223template <
typename Scalar>
230template <
typename Scalar>
235template <
typename Scalar>
242template <
typename Scalar>
246template <
typename Scalar>
254template <
typename Scalar>
258template <
typename Scalar>
262template <
typename Scalar>
266template <
typename Scalar>
273template <
typename Scalar>
276 return dt_bound_weight_cmd;
278template <
typename Scalar>
287template <
typename Scalar>
289 return dt_weight_cmd;
291template <
typename Scalar>
300template <
typename Scalar>
301const typename Eigen::Matrix<Scalar, 7, 1>&
310template <
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) {
317 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
319 if (
static_cast<std::size_t
>(
xref.size()) != 12) {
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) {
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];
335 pheuristic_[2 *
i + 1] =
l_feet(1,
i);
Definition quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
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