156 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
157 const Eigen::Ref<const typename MathBase::VectorXs>& x,
158 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
159 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
160 throw_pretty(
"Invalid argument: "
161 <<
"x has wrong dimension (it should be " +
162 std::to_string(state_->get_nx()) +
")");
164 if (
static_cast<std::size_t
>(u.size()) != nu_) {
165 throw_pretty(
"Invalid argument: "
166 <<
"u has wrong dimension (it should be " +
167 std::to_string(nu_) +
")");
174 d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
177 d->Lxx.block(0, 0, 6, 6).setZero();
179 (state_weights_.array() * state_weights_.array()).matrix();
180 for (
int j = 0; j < 4; j = j + 1) {
181 if (sh_ub_max_[j] >
Scalar(0.)) {
182 d->Lx(0, 0) += sh_weight * psh(0, j);
183 d->Lx(1, 0) += sh_weight * psh(1, j);
184 d->Lx(2, 0) += sh_weight * psh(2, j);
185 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
186 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
187 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
188 pshoulder_0(0, j) * psh(1, j));
190 d->Lxx(0, 0) += sh_weight;
191 d->Lxx(1, 1) += sh_weight;
192 d->Lxx(2, 2) += sh_weight;
193 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
194 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
195 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
196 pshoulder_0(0, j) * pshoulder_0(0, j));
198 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
199 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
201 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
202 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
204 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
205 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
206 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
207 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
209 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
210 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
215 for (
int i = 0; i < 4; i = i + 1) {
216 r = friction_weight_ * rub_max_.segment(6 * i, 6);
217 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
218 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
221 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
226 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
227 for (
int i = 0; i < 4; i = i + 1) {
228 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
229 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
230 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
231 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
235 (force_weights_.array() * force_weights_.array()).matrix();
240 if (implicit_integration) {
241 d->Fu.block(0, 0, 6, 12) << dt_ * B.block(6, 0, 6, 12);