104 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
105 const Eigen::Ref<const typename MathBase::VectorXs>& x,
106 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
107 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
108 throw_pretty(
"Invalid argument: "
109 <<
"x has wrong dimension (it should be " +
110 std::to_string(state_->get_nx()) +
")");
112 if (
static_cast<std::size_t
>(u.size()) != nu_) {
113 throw_pretty(
"Invalid argument: "
114 <<
"u has wrong dimension (it should be " +
115 std::to_string(nu_) +
")");
122 d->xnext.template head<12>() = x.head(12);
123 d->xnext.template segment<8>(12) = x.segment(12, 8) + B * u;
124 d->xnext.template tail<1>() = x.tail(1);
127 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
128 d->r.template segment<8>(12) = heuristicWeights.cwiseProduct(
132 d->r.template tail<4>() = step_weights_.cwiseProduct(u);
134 d->cost =
Scalar(0.5) * d->r.transpose() * d->r;
137 for (
int i = 0; i < 4; i++) {
139 rub_max_first_x.col(i) = x(20) * b_coeff_x0.col(i) +
140 x(20) * x(20) * b_coeff_x1.col(i) +
141 u(2 * i) * b_coeff_x2.col(i);
142 rub_max_first_y.col(i) = x(20) * b_coeff_y0.col(i) +
143 x(20) * x(20) * b_coeff_y1.col(i) +
144 u(2 * i + 1) * b_coeff_y2.col(i);
146 rub_max_first_2.col(i) =
147 rub_max_first_x.col(i).pow(2) + rub_max_first_y.col(i).pow(2) -
148 x(20) * x(20) * vlim * vlim * nb_nodes * nb_nodes;
150 rub_max_first_2.col(i).setZero();
155 (rub_max_first_2 >
Scalar(0.))
156 .
template cast<Scalar>();
157 rub_max_first_2 = rub_max_first_2.cwiseMax(
Scalar(0.));
159 for (
int i = 0; i < nb_alpha_; i++) {
160 d->cost += speed_weight *
Scalar(0.5) * rub_max_first_2.row(i).sum();
163 rub_max_ << u[0] * u[0] + u[1] * u[1] - beta_lim * x[20] * x[20],
164 u[2] * u[2] + u[3] * u[3] - beta_lim * x[20] * x[20];
167 (rub_max_.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
168 rub_max_ = rub_max_.cwiseMax(
Scalar(0.));
170 d->cost += speed_weight *
Scalar(0.5) * rub_max_.sum();
177 Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12);
178 cost_[1] =
Scalar(0.5) * d->r.segment(12, 8).transpose() *
180 cost_[2] =
Scalar(0.5) * d->r.tail(4).transpose() *
184 for (
int i = 0; i < 3; i++) {
185 cost_[3] += speed_weight *
Scalar(0.5) * rub_max_first_2.row(i).sum();
188 cost_[3] = speed_weight *
Scalar(0.5) * rub_max_.sum();
195 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
196 const Eigen::Ref<const typename MathBase::VectorXs>& x,
197 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
198 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
199 throw_pretty(
"Invalid argument: "
200 <<
"x has wrong dimension (it should be " +
201 std::to_string(state_->get_nx()) +
")");
203 if (
static_cast<std::size_t
>(u.size()) != nu_) {
204 throw_pretty(
"Invalid argument: "
205 <<
"u has wrong dimension (it should be " +
206 std::to_string(nu_) +
")");
218 d->Lx.template head<12>() =
219 (state_weights_.array() * d->r.template head<12>().array()).matrix();
220 d->Lx.template segment<8>(12) =
221 (heuristicWeights.array() * d->r.template segment<8>(12).array())
225 for (
int foot = 0; foot < 4; foot++) {
226 if (S_[foot] ==
Scalar(1)) {
227 for (
int i = 0; i < nb_alpha_; i++) {
228 if (rub_max_first_bool(i, foot)) {
231 (b_coeff_x0(i, foot) +
232 Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
233 rub_max_first_x(i, foot) +
235 (b_coeff_y0(i, foot) +
236 Scalar(2) * x(20) * b_coeff_y1(i, foot)) *
237 rub_max_first_y(i, foot) -
238 speed_weight * x(20) * vlim * vlim * nb_nodes * nb_nodes;
240 speed_weight * b_coeff_x2(i, foot) * rub_max_first_x(i, foot);
241 d->Lu(2 * foot + 1) +=
242 speed_weight * b_coeff_y2(i, foot) * rub_max_first_y(i, foot);
244 d->Luu(2 * foot, 2 * foot) +=
245 speed_weight * b_coeff_x2(i, foot) * b_coeff_x2(i, foot);
246 d->Luu(2 * foot + 1, 2 * foot + 1) +=
247 speed_weight * b_coeff_y2(i, foot) * b_coeff_y2(i, foot);
248 d->Lxu(20, 2 * foot) += speed_weight *
249 (b_coeff_x0(i, foot) +
250 Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
252 d->Lxu(20, 2 * foot + 1) +=
254 (b_coeff_y0(i, foot) +
255 Scalar(2) * x(20) * b_coeff_y1(i, foot)) *
259 std::pow(b_coeff_x0(i, foot) +
260 Scalar(2) * x(20) * b_coeff_x1(i, foot),
262 speed_weight *
Scalar(2) * b_coeff_x1(i, foot) *
263 rub_max_first_x(i, foot) +
265 std::pow(b_coeff_y0(i, foot) +
266 Scalar(2) * x(20) * b_coeff_x1(i, foot),
268 speed_weight *
Scalar(2) * b_coeff_y1(i, foot) *
269 rub_max_first_y(i, foot) -
270 speed_weight * vlim * vlim * nb_nodes * nb_nodes;
279 d->Lx.template tail<1>()
280 << -beta_lim * speed_weight * x(20) * rub_max_bool[0] -
281 beta_lim * speed_weight * x(20) * rub_max_bool[1];
283 d->Lu << speed_weight * u[0] * rub_max_bool[0],
284 speed_weight * u[1] * rub_max_bool[0],
285 speed_weight * u[2] * rub_max_bool[1],
286 speed_weight * u[3] * rub_max_bool[1];
288 d->Lxx(20, 20) = -beta_lim * speed_weight * rub_max_bool[0] -
289 beta_lim * speed_weight * rub_max_bool[1];
291 d->Luu.diagonal() << speed_weight * rub_max_bool[0],
292 speed_weight * rub_max_bool[0], speed_weight * rub_max_bool[1],
293 speed_weight * rub_max_bool[1];
296 d->Lu += (step_weights_.array() * d->r.template tail<4>().array()).matrix();
299 d->Lxx.diagonal().head(12) =
300 (state_weights_.array() * state_weights_.array()).matrix();
301 d->Lxx.diagonal().segment(12, 8) =
302 (heuristicWeights.array() * heuristicWeights.array()).matrix();
304 d->Luu.diagonal() += (step_weights_.array() * step_weights_.array()).matrix();
308 d->Fu.block(12, 0, 8, 8) = B;