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()) {
106 throw_pretty(
"Invalid argument: "
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_) {
111 throw_pretty(
"Invalid argument: "
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();
155 d->xnext.template head<12>() =
156 A.diagonal().cwiseProduct(x.block(0, 0, 12, 1)) + g;
157 d->xnext.template head<6>() =
158 d->xnext.template head<6>() +
159 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.block(6, 0, 6, 1));
160 d->xnext.template segment<6>(6) =
161 d->xnext.template segment<6>(6) + B.block(6, 0, 6, 12) * u;
162 d->xnext.template segment<8>(12) = x.segment(12, 8);
163 d->xnext.template tail<1>() = x.tail(1);
166 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
167 d->r.template segment<8>(12) =
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);
180 rub_max_ = (Fa_x_u - ub).cwiseMax(
Scalar(0.));
182 rub_max_dt << dt_min_ - x.tail(1), x.tail(1) - dt_max_;
184 (rub_max_dt.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
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()) {
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;