102 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
data,
103 const Eigen::Ref<const typename MathBase::VectorXs>&
x,
104 const Eigen::Ref<const typename MathBase::VectorXs>&
u) {
105 if (
static_cast<std::size_t
>(
x.size()) !=
state_->get_nx()) {
107 <<
"x has wrong dimension (it should be " +
108 std::to_string(
state_->get_nx()) +
")");
110 if (
static_cast<std::size_t
>(
u.size()) !=
nu_) {
112 <<
"u has wrong dimension (it should be " +
113 std::to_string(
nu_) +
")");
119 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() *
121 g[8] =
Scalar(-9.81) *
x.tail(1)[0];
124 for (
int i = 0;
i < 4;
i =
i + 1) {
126 if (gait(
i, 0) != 0) {
127 lever_tmp.head(2) =
x.block(12 + 2 *
i, 0, 2, 1);
128 lever_tmp += -
x.block(0, 0, 3, 1);
129 R_tmp <<
Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
130 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0],
Scalar(0.0);
132 B.block(9, 3 *
i, 3, 3) <<
x.tail(1)[0] * R * R_tmp;
133 B.block(6, 3 *
i, 3, 3).diagonal() <<
x.tail(1)[0] / mass,
134 x.tail(1)[0] / mass,
x.tail(1)[0] / mass;
137 psh.block(0,
i, 3, 1) <<
x(0) + pshoulder_0(0,
i) -
138 pshoulder_0(1,
i) *
x(5) -
x(12 + 2 *
i),
139 x(1) + pshoulder_0(1,
i) + pshoulder_0(0,
i) *
x(5) -
141 x(2) + pshoulder_0(1,
i) *
x(3) - pshoulder_0(0,
i) *
x(4);
144 psh.block(0,
i, 3, 1).setZero();
156 A.diagonal().cwiseProduct(
x.block(0, 0, 12, 1)) + g;
159 A.topRightCorner(6, 6).diagonal().cwiseProduct(
x.block(6, 0, 6, 1));
161 d->xnext.template
segment<6>(6) + B.block(6, 0, 6, 12) *
u;
166 d->r.template
head<12>() = state_weights_.cwiseProduct(
x.head(12) - xref_);
168 ((heuristicWeights.cwiseProduct(
x.segment(12, 8) - pheuristic_)).
array() *
172 d->r.template
tail<12>() = force_weights_.cwiseProduct(
u - uref_);
175 for (
int i = 0;
i < 4;
i =
i + 1) {
176 Fa_x_u.segment(6 *
i, 6) <<
u(3 *
i) - mu *
u(3 *
i + 2),
177 -
u(3 *
i) - mu *
u(3 *
i + 2),
u(3 *
i + 1) - mu *
u(3 *
i + 2),
178 -
u(3 *
i + 1) - mu *
u(3 *
i + 2), -
u(3 *
i + 2),
u(3 *
i + 2);
182 rub_max_dt << dt_min_ -
x.tail(1),
x.tail(1) - dt_max_;
185 rub_max_dt = rub_max_dt.cwiseMax(
Scalar(0.));
188 sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
189 psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
190 psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
191 psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
193 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
197 Scalar(0.5) *
d->r.segment(12, 8).transpose() *
d->r.segment(12, 8) +
198 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
200 ((last_position_weights_.cwiseProduct(
x.segment(12, 8) - pref_))
205 dt_bound_weight *
Scalar(0.5) * rub_max_dt.squaredNorm() +
206 x(20) *
Scalar(0.5) *
d->r.head(12).transpose() *
d->r.head(12) +
207 x(20) *
Scalar(0.5) *
d->r.tail(12).transpose() *
d->r.tail(12) +
208 sh_weight *
Scalar(0.5) * sh_ub_max_.sum();
211 cost_[0] =
x(20) *
Scalar(0.5) *
d->r.head(12).transpose() *
213 cost_[1] =
Scalar(0.5) *
d->r.segment(12, 8).transpose() *
215 cost_[2] =
x(20) *
Scalar(0.5) *
d->r.tail(12).transpose() *
217 cost_[3] = dt_bound_weight *
Scalar(0.5) *
218 rub_max_dt.squaredNorm();
220 ((last_position_weights_.cwiseProduct(
x.segment(12, 8) - pref_))
225 cost_[5] = friction_weight_ *
Scalar(0.5) *
226 rub_max_.squaredNorm();
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()) {
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_) {
242 <<
"u has wrong dimension (it should be " +
243 std::to_string(
nu_) +
")");
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]);
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);
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) =
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);
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();
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;