232 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
233 const Eigen::Ref<const typename MathBase::VectorXs>& x,
234 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
235 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
236 throw_pretty(
"Invalid argument: "
237 <<
"x has wrong dimension (it should be " +
238 std::to_string(state_->get_nx()) +
")");
240 if (
static_cast<std::size_t
>(u.size()) != nu_) {
241 throw_pretty(
"Invalid argument: "
242 <<
"u has wrong dimension (it should be " +
243 std::to_string(nu_) +
")");
251 d->Lx.template head<12>() =
253 (state_weights_.array() * d->r.template head<12>().array()).matrix();
254 d->Lx.template segment<8>(12) =
255 (heuristicWeights.array() * d->r.template segment<8>(12).array())
257 ((last_position_weights_.cwiseProduct(x.segment(12, 8) - pref_)).array() *
258 gait_double.array() * last_position_weights_.array())
260 d->Lx.template tail<1>() << dt_bound_weight *
261 (-rub_max_dt[0] + rub_max_dt[1]);
264 d->Lx.template tail<1>() +=
265 Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12) +
266 Scalar(0.5) * d->r.tail(12).transpose() * d->r.tail(12);
269 for (
int i = 0; i < 4; i = i + 1) {
270 r = friction_weight_ * rub_max_.segment(6 * i, 6);
271 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
272 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
277 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
281 d->Lxx.diagonal().head(12) =
282 x(20) * (state_weights_.array() * state_weights_.array()).matrix();
283 d->Lxx.diagonal().segment(12, 8) =
284 (gait_double.array() * heuristicWeights.array() *
285 heuristicWeights.array())
287 d->Lxx.diagonal().segment(12, 8) +=
288 (gait_double.array() * last_position_weights_.array() *
289 last_position_weights_.array())
292 d->Lxx.diagonal().tail(1) << dt_bound_weight * rub_max_dt_bool[0] +
293 dt_bound_weight * rub_max_dt_bool[1];
296 d->Lxx.col(20).head(12) =
297 (state_weights_.array() * d->r.template head<12>().array()).matrix();
298 d->Lxx.row(20).head(12) = d->Lxx.col(20).head(12);
300 for (
int j = 0; j < 4; j = j + 1) {
301 if (sh_ub_max_[j] >
Scalar(0.)) {
302 d->Lx(0, 0) += sh_weight * psh(0, j);
303 d->Lx(1, 0) += sh_weight * psh(1, j);
304 d->Lx(2, 0) += sh_weight * psh(2, j);
305 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
306 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
307 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
308 pshoulder_0(0, j) * psh(1, j));
310 d->Lx(12 + 2 * j, 0) += -sh_weight * psh(0, j);
311 d->Lx(12 + 2 * j + 1, 0) += -sh_weight * psh(1, j);
313 d->Lxx(0, 0) += sh_weight;
314 d->Lxx(1, 1) += sh_weight;
315 d->Lxx(2, 2) += sh_weight;
316 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
317 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
318 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
319 pshoulder_0(0, j) * pshoulder_0(0, j));
321 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight;
322 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight;
324 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
325 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
327 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
328 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
330 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
331 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
332 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
333 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
335 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
336 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
338 d->Lxx(0, 12 + 2 * j) += -sh_weight;
339 d->Lxx(12 + 2 * j, 0) += -sh_weight;
341 d->Lxx(5, 12 + 2 * j) += sh_weight * pshoulder_0(1, j);
342 d->Lxx(12 + 2 * j, 5) += sh_weight * pshoulder_0(1, j);
344 d->Lxx(1, 12 + 2 * j + 1) += -sh_weight;
345 d->Lxx(12 + 2 * j + 1, 1) += -sh_weight;
347 d->Lxx(5, 12 + 2 * j + 1) += -sh_weight * pshoulder_0(0, j);
348 d->Lxx(12 + 2 * j + 1, 5) += -sh_weight * pshoulder_0(0, j);
355 ((Fa_x_u - ub).array() >=
Scalar(0.)).matrix().template cast<Scalar>();
356 for (
int i = 0; i < 4; i = i + 1) {
357 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
358 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
359 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
360 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
364 x(20) * (force_weights_.array() * force_weights_.array()).matrix();
367 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
371 d->Fx.block(0, 0, 12, 12) << A;
372 d->Fx.block(12, 12, 8, 8) << Eigen::Matrix<Scalar, 8, 8>::Identity();
373 d->Fx.block(20, 20, 1, 1) <<
Scalar(1);
374 d->Fx.block(8, 20, 1, 1) << -
Scalar(9.81);
375 d->Fx.block(0, 20, 8, 1) << x.segment(6, 6);
377 for (
int i = 0; i < 4; i = i + 1) {
378 if (gait(i, 0) != 0) {
379 forces_3d = u.block(3 * i, 0, 3, 1);
380 d->Fx.block(9, 0, 3, 1) +=
381 -x.tail(1)[0] * R * (base_vector_x.cross(forces_3d));
382 d->Fx.block(9, 1, 3, 1) +=
383 -x.tail(1)[0] * R * (base_vector_y.cross(forces_3d));
384 d->Fx.block(9, 2, 3, 1) +=
385 -x.tail(1)[0] * R * (base_vector_z.cross(forces_3d));
387 d->Fx.block(9, 12 + 2 * i, 3, 1) +=
388 x.tail(1)[0] * R * (base_vector_x.cross(forces_3d));
389 d->Fx.block(9, 12 + 2 * i + 1, 3, 1) +=
390 x.tail(1)[0] * R * (base_vector_y.cross(forces_3d));
392 d->Fx.block(6, 20, 3, 1) += (1 / mass) * forces_3d;
395 lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
396 lever_tmp += -x.block(0, 0, 3, 1);
397 R_tmp <<
Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
398 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0],
Scalar(0.0);
399 d->Fx.block(9, 20, 3, 1) += R * R_tmp * forces_3d;
403 d->Fu.block(0, 0, 12, 12) << B;