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));