80 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
81 const Eigen::Ref<const typename MathBase::VectorXs>& x,
82 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
83 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
84 throw_pretty(
"Invalid argument: "
85 <<
"x has wrong dimension (it should be " +
86 std::to_string(state_->get_nx()) +
")");
88 if (
static_cast<std::size_t
>(u.size()) != nu_) {
89 throw_pretty(
"Invalid argument: "
90 <<
"u has wrong dimension (it should be " +
91 std::to_string(nu_) +
")");
98 for (
int i = 0; i < 4; i = i + 1) {
99 if (gait(i, 0) != 0) {
100 lever_tmp = lever_arms.block(0, i, 3, 1) - x.block(0, 0, 3, 1);
101 R_tmp << 0.0, -lever_tmp[2], lever_tmp[1], lever_tmp[2], 0.0,
102 -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0;
103 B.block(9, 3 * i, 3, 3) << dt_ * I_inv * R_tmp;
106 psh.block(0, i, 3, 1) << x[0] - offset_com(0, 0) + pshoulder_0(0, i) -
107 pshoulder_0(1, i) * x[5] - lever_arms(0, i),
108 x[1] - offset_com(1, 0) + pshoulder_0(1, i) +
109 pshoulder_0(0, i) * x[5] - lever_arms(1, i),
110 x[2] - offset_com(2, 0) + pshoulder_0(1, i) * x[3] -
111 pshoulder_0(0, i) * x[4];
114 psh.block(0, i, 3, 1).setZero();
119 d->xnext << A.diagonal().cwiseProduct(x) + g;
120 d->xnext.template head<6>() =
121 d->xnext.template head<6>() +
122 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.tail(6));
123 d->xnext.template tail<6>() =
124 d->xnext.template tail<6>() + B.block(6, 0, 6, 12) * u;
127 d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
128 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
131 for (
int i = 0; i < 4; i = i + 1) {
132 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
133 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
134 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
136 rub_max_ = (Fa_x_u - ub).cwiseMax(
Scalar(0.));
139 sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
140 psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
141 psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
142 psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
144 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
150 d->cost = 0.5 * d->r.transpose() * d->r +
151 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
152 sh_weight *
Scalar(0.5) * sh_ub_max_.sum();
157 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
158 const Eigen::Ref<const typename MathBase::VectorXs>& x,
159 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
160 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
161 throw_pretty(
"Invalid argument: "
162 <<
"x has wrong dimension (it should be " +
163 std::to_string(state_->get_nx()) +
")");
165 if (
static_cast<std::size_t
>(u.size()) != nu_) {
166 throw_pretty(
"Invalid argument: "
167 <<
"u has wrong dimension (it should be " +
168 std::to_string(nu_) +
")");
176 d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
179 d->Lxx.block(0, 0, 6, 6).setZero();
181 (state_weights_.array() * state_weights_.array()).matrix();
184 for (
int j = 0; j < 4; j = j + 1) {
185 if (sh_ub_max_[j] >
Scalar(0.)) {
186 d->Lx(0, 0) += sh_weight * psh(0, j);
187 d->Lx(1, 0) += sh_weight * psh(1, j);
188 d->Lx(2, 0) += sh_weight * psh(2, j);
189 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
190 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
191 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
192 pshoulder_0(0, j) * psh(1, j));
194 d->Lxx(0, 0) += sh_weight;
195 d->Lxx(1, 1) += sh_weight;
196 d->Lxx(2, 2) += sh_weight;
197 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
198 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
199 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
200 pshoulder_0(0, j) * pshoulder_0(0, j));
202 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
203 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
205 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
206 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
208 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
209 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
210 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
211 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
213 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
214 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
219 for (
int i = 0; i < 4; i = i + 1) {
220 r = friction_weight_ * rub_max_.segment(6 * i, 6);
221 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
222 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
225 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
230 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
231 for (
int i = 0; i < 4; i = i + 1) {
232 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
233 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
234 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
235 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
239 (force_weights_.array() * force_weights_.array()).matrix();
244 for (
int i = 0; i < 4; i = i + 1) {
245 if (gait(i, 0) != 0) {
246 forces_3d = u.block(3 * i, 0, 3, 1);
247 d->Fx.block(9, 0, 3, 1) +=
248 -dt_ * I_inv * (base_vector_x.cross(forces_3d));
249 d->Fx.block(9, 1, 3, 1) +=
250 -dt_ * I_inv * (base_vector_y.cross(forces_3d));
251 d->Fx.block(9, 2, 3, 1) +=
252 -dt_ * I_inv * (base_vector_z.cross(forces_3d));
452 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
453 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
454 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
455 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
456 throw_pretty(
"Invalid argument: "
457 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
459 if (
static_cast<std::size_t
>(xref.size()) != state_->get_nx()) {
460 throw_pretty(
"Invalid argument: "
461 <<
"Weights vector has wrong dimension (it should be " +
462 std::to_string(state_->get_nx()) +
")");
464 if (
static_cast<std::size_t
>(S.size()) != 4) {
465 throw_pretty(
"Invalid argument: "
466 <<
"S vector has wrong dimension (it should be 4x1)");
474 if (relative_forces) {
475 for (
int i = 0; i < 4; i = i + 1) {
477 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
482 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), 0, sin(xref(5, 0)),
483 cos(xref(5, 0)), 0, 0, 0, 1.0;
485 I_inv = (R_tmp.transpose() * gI * R_tmp).inverse();
486 lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
488 for (
int i = 0; i < 4; i = i + 1) {
491 ub(6 * i + 4) = -min_fz_in_contact;
494 B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
507 B.block(6, 3 * i, 3, 3).setZero();
508 B.block(9, 3 * i, 3, 3).setZero();