10 #include <crocoddyl/core/utils/exception.hpp>
16 using namespace crocoddyl;
17 template <
typename Scalar>
18 IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
19 boost::shared_ptr<DifferentialActionModelAbstract> model,
20 const Scalar& time_step,
const bool& with_cost_residual,
const Scalar& fc,
21 const bool& tau_plus_integration,
const int& filter,
22 const bool& is_terminal)
23 : Base(model->get_state(), model->get_nu(),
24 model->get_nr() + 2 * model->get_nu()),
26 time_step_(time_step),
27 time_step2_(time_step * time_step),
28 with_cost_residual_(with_cost_residual),
30 tau_plus_integration_(tau_plus_integration),
32 ny_(model->get_state()->get_nx() + model->get_nu()),
33 enable_integration_(true),
35 is_terminal_(is_terminal) {
36 Base::set_u_lb(differential_->get_u_lb());
37 Base::set_u_ub(differential_->get_u_ub());
38 if (time_step_ < Scalar(0.)) {
39 time_step_ = Scalar(1e-3);
40 time_step2_ = time_step_ * time_step_;
41 std::cerr <<
"Warning: dt should be positive, set to 1e-3" << std::endl;
43 if (time_step == Scalar(0.) || is_terminal_ ==
true) {
44 enable_integration_ =
false;
49 boost::shared_ptr<StateMultibody> state =
50 boost::static_pointer_cast<StateMultibody>(model->get_state());
51 pin_model_ = state->get_pinocchio();
53 state_ = boost::make_shared<StateLPF>(pin_model_, model->get_nu());
55 VectorXs wlb = state_->get_lb().tail(nw_);
56 VectorXs wub = state_->get_ub().tail(nw_);
57 activation_model_w_lim_ = boost::make_shared<ActivationModelQuadraticBarrier>(
58 ActivationBounds(wlb, wub));
64 template <
typename Scalar>
65 IntegratedActionModelLPFTpl<Scalar>::~IntegratedActionModelLPFTpl() {}
67 template <
typename Scalar>
68 void IntegratedActionModelLPFTpl<Scalar>::calc(
69 const boost::shared_ptr<ActionDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w) {
71 const std::size_t& nv = differential_->get_state()->get_nv();
72 const std::size_t& nq = differential_->get_state()->get_nq();
73 const std::size_t& nx = differential_->get_state()->get_nx();
74 const std::size_t& ny =
75 boost::static_pointer_cast<StateLPF>(state_)->get_ny();
76 const std::size_t& nu = differential_->get_nu();
77 const std::size_t& nw =
78 boost::static_pointer_cast<StateLPF>(state_)->get_nw();
80 if (
static_cast<std::size_t
>(y.size()) != ny) {
81 throw_pretty(
"Invalid argument: "
82 <<
"y has wrong dimension (it should be " +
83 std::to_string(ny) +
")");
85 if (
static_cast<std::size_t
>(w.size()) != nw) {
86 throw_pretty(
"Invalid argument: "
87 <<
"w has wrong dimension (it should be " +
88 std::to_string(nw) +
")");
92 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
94 const Eigen::Ref<const VectorXs>& x = y.head(nx);
95 const Eigen::Ref<const VectorXs>& tau = y.tail(nu);
97 if (
static_cast<std::size_t
>(x.size()) != nx) {
98 throw_pretty(
"Invalid argument: "
99 <<
"x has wrong dimension (it should be " +
100 std::to_string(nx) +
")");
102 if (
static_cast<std::size_t
>(tau.size()) != nu) {
103 throw_pretty(
"Invalid argument: "
104 <<
"tau has wrong dimension (it should be " +
105 std::to_string(nu) +
")");
108 if (
static_cast<std::size_t
>(d->Fy.rows()) !=
109 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
112 <<
"Fy.rows() has wrong dimension (it should be " +
114 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
117 if (
static_cast<std::size_t
>(d->Fy.cols()) !=
118 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
121 <<
"Fy.cols() has wrong dimension (it should be " +
123 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
126 if (
static_cast<std::size_t
>(d->Fw.cols()) !=
127 boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
130 <<
"Fw.cols() has wrong dimension (it should be " +
132 boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
135 if (
static_cast<std::size_t
>(d->r.size()) !=
136 differential_->get_nr() +
137 2 * boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
140 <<
"r has wrong dimension (it should be " +
142 differential_->get_nr() +
143 2 * boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
146 if (
static_cast<std::size_t
>(d->Ly.size()) !=
147 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
150 <<
"Ly has wrong dimension (it should be " +
152 boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
155 if (
static_cast<std::size_t
>(d->Lw.size()) !=
156 boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
159 <<
"Lw has wrong dimension (it should be " +
161 boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
166 if (!tau_plus_integration_) {
168 differential_->calc(d->differential, x,
172 const Eigen::Ref<const VectorXs>& tau_plus =
173 alpha_ * tau + (1 - alpha_) * w;
174 differential_->calc(d->differential, x,
179 VectorXs res_w_reg, res_w_lim;
184 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
186 const VectorXs& a = d->differential->xout;
187 d->dy.head(nv).noalias() =
188 v * time_step_ + a * time_step2_;
189 d->dy.segment(nq, nv).noalias() = a * time_step_;
190 d->dy.tail(nu).noalias() =
191 (1 - alpha_) * (w - tau);
195 d->cost = time_step_ * d->differential->cost;
198 res_w_reg = w - wref_;
199 d->cost += Scalar(0.5 * time_step_ * wreg_ * res_w_reg.transpose() *
203 activation_model_w_lim_->calc(
207 Scalar(0.5 * time_step_ * wlim_ * d->activation->a_value);
215 d->cost = d->differential->cost;
219 if (with_cost_residual_) {
220 d->r.head(differential_->get_nr()) = d->differential->r;
224 d->r.segment(differential_->get_nr(), nw) = res_w_reg;
227 d->r.tail(nw) = res_w_lim;
233 template <
typename Scalar>
234 void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
235 const boost::shared_ptr<ActionDataAbstract>& data,
236 const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w) {
237 const std::size_t& nv = differential_->get_state()->get_nv();
238 const std::size_t& nq = differential_->get_state()->get_nq();
239 const std::size_t& nx = differential_->get_state()->get_nx();
240 const std::size_t& ndx = differential_->get_state()->get_ndx();
241 const std::size_t& ny =
242 boost::static_pointer_cast<StateLPF>(state_)->get_ny();
243 const std::size_t& nu = differential_->get_nu();
244 const std::size_t& nw =
245 boost::static_pointer_cast<StateLPF>(state_)->get_nw();
247 if (
static_cast<std::size_t
>(y.size()) != ny) {
248 throw_pretty(
"Invalid argument: "
249 <<
"y has wrong dimension (it should be " +
250 std::to_string(ny) +
")");
252 if (
static_cast<std::size_t
>(w.size()) != nw) {
253 throw_pretty(
"Invalid argument: "
254 <<
"w has wrong dimension (it should be " +
255 std::to_string(nw) +
")");
259 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
263 const Eigen::Ref<const VectorXs>& x = y.head(nx);
264 const Eigen::Ref<const VectorXs>& tau = y.tail(nu);
267 if (!tau_plus_integration_) {
269 differential_->calcDiff(d->differential, x, tau);
273 activation_model_w_lim_->calcDiff(d->activation, w);
279 const MatrixXs& da_dx = d->differential->Fx;
280 const MatrixXs& da_du = d->differential->Fu;
282 d->Fy.block(0, 0, nv, ndx).noalias() = da_dx * time_step2_;
283 d->Fy.block(nv, 0, nv, ndx).noalias() = da_dx * time_step_;
284 d->Fy.block(0, nv, nv, nv).diagonal().array() += Scalar(time_step_);
285 d->Fy.block(0, ndx, nv, nu).noalias() = da_du * time_step2_;
286 d->Fy.block(nv, ndx, nv, nu).noalias() = da_du * time_step_;
287 d->Fy.bottomRightCorner(nu, nu).diagonal().array() = Scalar(alpha_);
288 state_->JintegrateTransport(y, d->dy, d->Fy, second);
290 y, d->dy, d->Fy, d->Fy, first,
292 d->Fy.bottomRightCorner(nu, nu).diagonal().array() -=
296 d->Fw.bottomRows(nu).diagonal().array() = Scalar(1 - alpha_);
297 state_->JintegrateTransport(y, d->dy, d->Fw, second);
299 d->Ly.head(ndx).noalias() = time_step_ * d->differential->Lx;
300 d->Ly.tail(nu).noalias() = time_step_ * d->differential->Lu;
301 d->Lyy.topLeftCorner(ndx, ndx).noalias() =
302 time_step_ * d->differential->Lxx;
303 d->Lyy.block(0, ndx, ndx, nu).noalias() =
304 time_step_ * d->differential->Lxu;
305 d->Lyy.block(ndx, 0, nu, ndx).noalias() =
306 time_step_ * d->differential->Lxu.transpose();
307 d->Lyy.bottomRightCorner(nu, nu).noalias() =
308 time_step_ * d->differential->Luu;
311 d->Lw.noalias() = time_step_ * wreg_ *
312 d->r.segment(differential_->get_nr(), nw);
313 d->Lww.diagonal().array() = Scalar(time_step_ * wreg_);
316 d->Lw.noalias() += time_step_ * wlim_ * d->activation->Ar;
318 time_step_ * wlim_ * d->activation->Arr.diagonal();
324 state_->Jintegrate(y, d->dy, d->Fy, d->Fy);
326 d->Ly.head(ndx).noalias() = d->differential->Lx;
327 d->Ly.tail(nu).noalias() = d->differential->Lu;
328 d->Lyy.topLeftCorner(ndx, ndx).noalias() = d->differential->Lxx;
329 d->Lyy.block(0, ndx, ndx, nu).noalias() = d->differential->Lxu;
330 d->Lyy.block(ndx, 0, nu, ndx).noalias() =
331 d->differential->Lxu.transpose();
332 d->Lyy.bottomRightCorner(nu, nu).noalias() = d->differential->Luu;
343 const Eigen::Ref<const VectorXs>& tau_plus =
344 alpha_ * tau + (1 - alpha_) * w;
346 differential_->calcDiff(d->differential, x, tau_plus);
349 activation_model_w_lim_->calcDiff(d->activation, w);
352 if (enable_integration_) {
353 const MatrixXs& da_dx = d->differential->Fx;
354 const MatrixXs& da_du = d->differential->Fu;
355 d->Fy.block(0, 0, nv, ndx).noalias() = da_dx * time_step2_;
356 d->Fy.block(nv, 0, nv, ndx).noalias() = da_dx * time_step_;
357 d->Fy.block(0, ndx, nv, nu).noalias() =
358 alpha_ * alpha_ * da_du * time_step2_;
359 d->Fy.block(nv, ndx, nv, nu).noalias() = alpha_ * da_du * time_step_;
360 d->Fy.block(0, nq, nv, nv).diagonal().array() +=
365 d->Fy.bottomRightCorner(nu, nu).diagonal().array() = Scalar(alpha_);
366 d->Fw.topRows(nv).noalias() = da_du * time_step2_ * (1 - alpha_);
367 d->Fw.block(nv, 0, nv, nu).noalias() = da_du * time_step_ * (1 - alpha_);
368 d->Fw.bottomRows(nv).diagonal().array() = Scalar(1 - alpha_);
369 state_->JintegrateTransport(y, d->dy, d->Fy, second);
370 state_->Jintegrate(y, d->dy, d->Fy, d->Fy, first,
372 d->Fy.bottomRightCorner(nu, nu).diagonal().array() -=
374 state_->JintegrateTransport(y, d->dy, d->Fw, second);
375 d->Ly.head(ndx).noalias() = time_step_ * d->differential->Lx;
376 d->Ly.tail(nu).noalias() = alpha_ * time_step_ * d->differential->Lu;
377 d->Lw.noalias() = (1 - alpha_) * time_step_ * d->differential->Lu;
378 d->Lyy.topLeftCorner(ndx, ndx).noalias() =
379 time_step_ * d->differential->Lxx;
380 d->Lyy.block(0, ndx, ndx, nu).noalias() =
381 alpha_ * time_step_ * d->differential->Lxu;
382 d->Lyy.block(ndx, 0, nu, ndx).noalias() =
383 alpha_ * time_step_ * d->differential->Lxu.transpose();
384 d->Lyy.bottomRightCorner(nu, nu).noalias() =
385 alpha_ * alpha_ * time_step_ * d->differential->Luu;
386 d->Lyw.topRows(ndx).noalias() =
387 (1 - alpha_) * time_step_ * d->differential->Lxu;
388 d->Lyw.bottomRows(nu).noalias() =
389 (1 - alpha_) * alpha_ * time_step_ * d->differential->Luu;
391 (1 - alpha_) * (1 - alpha_) * time_step_ * d->differential->Luu;
396 d->Lw.noalias() = time_step_ * wreg_ *
397 d->r.segment(differential_->get_nr(), nw);
398 d->Lw.noalias() += time_step_ * wlim_ * d->activation->Ar;
399 d->Lww.diagonal().array() = Scalar(time_step_ * wreg_);
401 time_step_ * wlim_ * d->activation->Arr.diagonal();
407 d->Ly.head(ndx).noalias() = d->differential->Lx;
408 d->Ly.tail(nu).noalias() = alpha_ * d->differential->Lu;
409 d->Lw.noalias() = (1 - alpha_) * d->differential->Lu;
410 d->Lyy.topLeftCorner(ndx, ndx).noalias() = d->differential->Lxx;
411 d->Lyy.block(0, ndx, ndx, nu).noalias() = alpha_ * d->differential->Lxu;
412 d->Lyy.block(ndx, 0, nu, ndx).noalias() =
413 alpha_ * d->differential->Lxu.transpose();
414 d->Lyy.bottomRightCorner(nu, nu).noalias() =
415 alpha_ * alpha_ * d->differential->Luu;
416 d->Lyw.topRows(ndx).noalias() = (1 - alpha_) * d->differential->Lxu;
417 d->Lyw.bottomRows(nu).noalias() =
418 (1 - alpha_) * alpha_ * d->differential->Luu;
419 d->Lww.noalias() = (1 - alpha_) * (1 - alpha_) * d->differential->Luu;
424 wreg_ * d->r.segment(differential_->get_nr(), nw);
425 d->Lw.noalias() += wlim_ * d->activation->Ar;
426 d->Lww.diagonal().array() += Scalar(wreg_);
427 d->Lww.diagonal() += wlim_ * d->activation->Arr.diagonal();
433 template <
typename Scalar>
434 boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
435 IntegratedActionModelLPFTpl<Scalar>::createData() {
436 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
439 template <
typename Scalar>
440 bool IntegratedActionModelLPFTpl<Scalar>::checkData(
441 const boost::shared_ptr<ActionDataAbstract>& data) {
442 boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
444 return differential_->checkData(d->differential);
450 template <
typename Scalar>
451 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
452 IntegratedActionModelLPFTpl<Scalar>::get_differential()
const {
453 return differential_;
456 template <
typename Scalar>
457 const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_dt()
const {
461 template <
typename Scalar>
462 const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_fc()
const {
466 template <
typename Scalar>
467 void IntegratedActionModelLPFTpl<Scalar>::set_dt(
const Scalar& dt) {
469 throw_pretty(
"Invalid argument: "
470 <<
"dt has positive value");
473 time_step2_ = dt * dt;
476 template <
typename Scalar>
477 void IntegratedActionModelLPFTpl<Scalar>::set_fc(
const Scalar& fc) {
480 throw_pretty(
"Invalid argument: "
481 <<
"fc must be positive");
487 template <
typename Scalar>
488 void IntegratedActionModelLPFTpl<Scalar>::compute_alpha(
const Scalar& fc) {
490 if (fc > 0 && time_step_ != 0) {
491 const Scalar& pi = 3.14159;
494 alpha_ = exp(-2. * pi * fc * time_step_);
498 double omega = 1 / (2. * pi * time_step_ * fc);
499 alpha_ = omega / (omega + 1);
503 double y = cos(2. * pi * time_step_ * fc);
504 alpha_ = 1 - (y - 1 + sqrt(y * y - 4 * y + 3));
511 template <
typename Scalar>
512 void IntegratedActionModelLPFTpl<Scalar>::set_differential(
513 boost::shared_ptr<DifferentialActionModelAbstract> model) {
514 const std::size_t& nu = model->get_nu();
517 unone_ = VectorXs::Zero(nu_);
519 nr_ = model->get_nr() + 2 * nu;
520 state_ = boost::static_pointer_cast<StateLPF>(
522 differential_ = model;
523 Base::set_u_lb(differential_->get_u_lb());
524 Base::set_u_ub(differential_->get_u_ub());
527 template <
typename Scalar>
528 void IntegratedActionModelLPFTpl<Scalar>::set_control_reg_cost(
529 const Scalar& weight,
const VectorXs& ref) {
531 throw_pretty(
"cost weight is positive ");
537 template <
typename Scalar>
538 void IntegratedActionModelLPFTpl<Scalar>::set_control_lim_cost(
539 const Scalar& weight) {
541 throw_pretty(
"cost weight is positive ");
546 template <
typename Scalar>
547 void IntegratedActionModelLPFTpl<Scalar>::quasiStatic(
548 const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
549 const Eigen::Ref<const VectorXs>& x,
const std::size_t& maxiter,
551 if (
static_cast<std::size_t
>(u.size()) != nu_) {
552 throw_pretty(
"Invalid argument: "
553 <<
"u has wrong dimension (it should be " +
554 std::to_string(nu_) +
")");
556 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
557 throw_pretty(
"Invalid argument: "
558 <<
"x has wrong dimension (it should be " +
559 std::to_string(state_->get_nx()) +
")");
563 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
565 differential_->quasiStatic(d->differential, u, x, maxiter, tol);