lpf.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <cmath>
10 #include <crocoddyl/core/utils/exception.hpp>
11 #include <iostream>
12 
13 #include "lpf.hpp"
14 
15 namespace sobec {
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()),
25  differential_(model),
26  time_step_(time_step),
27  time_step2_(time_step * time_step),
28  with_cost_residual_(with_cost_residual),
29  fc_(fc),
30  tau_plus_integration_(tau_plus_integration),
31  nw_(model->get_nu()),
32  ny_(model->get_state()->get_nx() + model->get_nu()),
33  enable_integration_(true),
34  filter_(filter),
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;
42  }
43  if (time_step == Scalar(0.) || is_terminal_ == true) {
44  enable_integration_ = false;
45  }
46  // Compute alpha parameter from fc_
47  compute_alpha(fc_);
48  // Downcast DAM state (abstract --> multibody)
49  boost::shared_ptr<StateMultibody> state =
50  boost::static_pointer_cast<StateMultibody>(model->get_state());
51  pin_model_ = state->get_pinocchio();
52  // Instantiate stateLPF using pinocchio model of DAM state
53  state_ = boost::make_shared<StateLPF>(pin_model_, model->get_nu());
54  // init barrier for lim cost
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));
59  // cost weights are zero by default
60  wreg_ = Scalar(0.);
61  wlim_ = Scalar(0.);
62 }
63 
64 template <typename Scalar>
65 IntegratedActionModelLPFTpl<Scalar>::~IntegratedActionModelLPFTpl() {}
66 
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();
79 
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) + ")");
84  }
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) + ")");
89  }
90 
91  // Static casting the data
92  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
93  // Extract x=(q,v) and tau from augmented state y
94  const Eigen::Ref<const VectorXs>& x = y.head(nx); // get q,v_q
95  const Eigen::Ref<const VectorXs>& tau = y.tail(nu); // get tau_q
96 
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) + ")");
101  }
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) + ")");
106  }
107 
108  if (static_cast<std::size_t>(d->Fy.rows()) !=
109  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
110  throw_pretty(
111  "Invalid argument: "
112  << "Fy.rows() has wrong dimension (it should be " +
113  std::to_string(
114  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
115  ")");
116  }
117  if (static_cast<std::size_t>(d->Fy.cols()) !=
118  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
119  throw_pretty(
120  "Invalid argument: "
121  << "Fy.cols() has wrong dimension (it should be " +
122  std::to_string(
123  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
124  ")");
125  }
126  if (static_cast<std::size_t>(d->Fw.cols()) !=
127  boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
128  throw_pretty(
129  "Invalid argument: "
130  << "Fw.cols() has wrong dimension (it should be " +
131  std::to_string(
132  boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
133  ")");
134  }
135  if (static_cast<std::size_t>(d->r.size()) !=
136  differential_->get_nr() +
137  2 * boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
138  throw_pretty(
139  "Invalid argument: "
140  << "r has wrong dimension (it should be " +
141  std::to_string(
142  differential_->get_nr() +
143  2 * boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
144  ")");
145  }
146  if (static_cast<std::size_t>(d->Ly.size()) !=
147  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
148  throw_pretty(
149  "Invalid argument: "
150  << "Ly has wrong dimension (it should be " +
151  std::to_string(
152  boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) +
153  ")");
154  }
155  if (static_cast<std::size_t>(d->Lw.size()) !=
156  boost::static_pointer_cast<StateLPF>(state_)->get_nw()) {
157  throw_pretty(
158  "Invalid argument: "
159  << "Lw has wrong dimension (it should be " +
160  std::to_string(
161  boost::static_pointer_cast<StateLPF>(state_)->get_nw()) +
162  ")");
163  }
164 
165  // Compute acceleration and cost (DAM, i.e. CT model)
166  if (!tau_plus_integration_) { // TAU INTEGRATION
167  // a,cost = DAM(x,tau)
168  differential_->calc(d->differential, x,
169  tau); // get a_q, cost = DAM(q, v_q, tau_q)
170  } else { // TAU PLUS INTEGRATION
171  // a,cost = DAM(x,tau+)
172  const Eigen::Ref<const VectorXs>& tau_plus =
173  alpha_ * tau + (1 - alpha_) * w; // get tau_q+ from (tau_q, w)
174  differential_->calc(d->differential, x,
175  tau_plus); // get a_q, cost = DAM(q, v_q, tau_q+)
176  } // DAM.calc
177 
178  // To store residuals of hard-coded costs on w regularization and w limit
179  VectorXs res_w_reg, res_w_lim;
180 
181  // INTEGRATE (only for running nodes)
182  if (!is_terminal_) {
183  // Computing the next state x+ = x + dx and cost+ = dt*cost
184  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
185  v = x.tail(nv);
186  const VectorXs& a = d->differential->xout;
187  d->dy.head(nv).noalias() =
188  v * time_step_ + a * time_step2_; // get dq(a_q, dt)
189  d->dy.segment(nq, nv).noalias() = a * time_step_; // get dv_q(a_q, dt)
190  d->dy.tail(nu).noalias() =
191  (1 - alpha_) * (w - tau); // get dtau_q(w, tau, alpha)
192  state_->integrate(
193  y, d->dy,
194  d->ynext); // integrate using stateLPF rule : tau+ = tau + dtau(tau, w)
195  d->cost = time_step_ * d->differential->cost; // get cost+ from cost
196  // Add hard-coded cost on unfiltered torque a[r(w)]
197  if (wreg_ != 0) {
198  res_w_reg = w - wref_;
199  d->cost += Scalar(0.5 * time_step_ * wreg_ * res_w_reg.transpose() *
200  res_w_reg); // w reg
201  }
202  if (wlim_ != 0) {
203  activation_model_w_lim_->calc(
204  d->activation, w); // Compute limit cost torque residual of w
205  res_w_lim = w;
206  d->cost +=
207  Scalar(0.5 * time_step_ * wlim_ * d->activation->a_value); // w lim
208  }
209  } // running model
210 
211  // For TERMINAL node, no integration & no cost on control w
212  else {
213  d->dy.setZero();
214  d->ynext = y;
215  d->cost = d->differential->cost;
216  } // terminal model
217 
218  // Update RESIDUAL
219  if (with_cost_residual_) {
220  d->r.head(differential_->get_nr()) = d->differential->r;
221  // Add unfiltered torque RESIDUAL(w) for RUNNING MODELS
222  if (!is_terminal_) {
223  if (wreg_ != 0) {
224  d->r.segment(differential_->get_nr(), nw) = res_w_reg; // w reg
225  } // wreg_ != 0
226  if (wlim_ != 0) {
227  d->r.tail(nw) = res_w_lim; // w lim
228  } // wlim_ != 0
229  } // running residual
230  } // update residual
231 } // calc
232 
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();
246 
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) + ")");
251  }
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) + ")");
256  }
257 
258  // Static casting the data
259  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
260 
261  // Computing the derivatives for the time-continuous model (i.e. differential
262  // model)
263  const Eigen::Ref<const VectorXs>& x = y.head(nx); // get q,v_q
264  const Eigen::Ref<const VectorXs>& tau = y.tail(nu); // get tau_q
265 
266  // TAU INTEGRATION
267  if (!tau_plus_integration_) {
268  // Get partials of CT model a_q ('f'), cost w.r.t. (q,v,tau)
269  differential_->calcDiff(d->differential, x, tau);
270  // Get cost lim w derivatives
271  if (!is_terminal_) {
272  if (wlim_ != 0) {
273  activation_model_w_lim_->calcDiff(d->activation, w);
274  } // wlim_ != 0
275  }
276 
277  // Fill RUNNING MODELS partials of (y+,cost+) w.r.t. (y,w)
278  if (!is_terminal_) {
279  const MatrixXs& da_dx = d->differential->Fx;
280  const MatrixXs& da_du = d->differential->Fu;
281  // d(x+)/dy
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);
289  state_->Jintegrate(
290  y, d->dy, d->Fy, d->Fy, first,
291  addto); // add identity to Fx = d(x+dx)/dx = d(q,v)/d(q,v)
292  d->Fy.bottomRightCorner(nu, nu).diagonal().array() -=
293  Scalar(1.); // remove identity from Ftau (due to stateLPF.Jintegrate)
294  // d(x+)/dw
295  d->Fw.setZero();
296  d->Fw.bottomRows(nu).diagonal().array() = Scalar(1 - alpha_);
297  state_->JintegrateTransport(y, d->dy, d->Fw, second);
298  // d(cost+)/dy
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;
309  // Partials of hard-coded cost+(wreg) & cost+(wlim) w.r.t. (y,w)
310  if (wreg_ != 0) {
311  d->Lw.noalias() = time_step_ * wreg_ *
312  d->r.segment(differential_->get_nr(), nw); // w reg
313  d->Lww.diagonal().array() = Scalar(time_step_ * wreg_); // w reg
314  } // wreg !=0
315  if (wlim_ != 0) {
316  d->Lw.noalias() += time_step_ * wlim_ * d->activation->Ar; // w lim
317  d->Lww.diagonal() +=
318  time_step_ * wlim_ * d->activation->Arr.diagonal(); // w lim
319  } // wlim !=0
320  } // !is_terminal // running model
321 
322  // Fill TERMINAL MODEL partials of ( a(yT), cost(yT) ) w.r.t. y
323  else {
324  state_->Jintegrate(y, d->dy, d->Fy, d->Fy);
325  d->Fw.setZero();
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;
333  d->Lw.setZero();
334  d->Lww.setZero();
335  d->Lyw.setZero();
336  } // terminal model
337 
338  } // tau integration
339 
340  // TAU PLUS INTEGRATION
341  else {
342  // get tau_q+ from (tau_q, w)
343  const Eigen::Ref<const VectorXs>& tau_plus =
344  alpha_ * tau + (1 - alpha_) * w;
345  // Get partials of CT model a_q ('f'), cost w.r.t. (q,v,tau+)
346  differential_->calcDiff(d->differential, x, tau_plus);
347  // Get cost lim w
348  if (!is_terminal_) {
349  activation_model_w_lim_->calcDiff(d->activation, w);
350  }
351  // Fill out partials of IAM
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() +=
361  Scalar(time_step_); // dt*identity top row middle col (eq.
362  // Jsecond = d(xnext)/d(dx))
363  // d->Fy.topLeftCorner(nx, nx).diagonal().array() += Scalar(1.); //
364  // managed by Jintegrate (eq. Jsecond = d(xnext)/d(dx))
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); // it this correct?
370  state_->Jintegrate(y, d->dy, d->Fy, d->Fy, first,
371  addto); // for d(x+dx)/d(x)
372  d->Fy.bottomRightCorner(nu, nu).diagonal().array() -=
373  Scalar(1.); // remove identity from Ftau (due to stateLPF.Jintegrate)
374  state_->JintegrateTransport(y, d->dy, d->Fw, second); // it this correct?
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;
390  d->Lww.noalias() =
391  (1 - alpha_) * (1 - alpha_) * time_step_ * d->differential->Luu;
392  // Add partials related to unfiltered torque costs w_reg, w_lim (only for
393  // running models)
394  if (!is_terminal_) {
395  // Torque reg and lim
396  d->Lw.noalias() = time_step_ * wreg_ *
397  d->r.segment(differential_->get_nr(), nw); // w reg
398  d->Lw.noalias() += time_step_ * wlim_ * d->activation->Ar; // w lim
399  d->Lww.diagonal().array() = Scalar(time_step_ * wreg_); // reg
400  d->Lww.diagonal() +=
401  time_step_ * wlim_ * d->activation->Arr.diagonal(); // lim
402  }
403 
404  } else {
405  // state_->Jintegrate(y, d->dy, d->Fy, d->Fy);
406  d->Fw.setZero();
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;
420  // Add partials related to unfiltered torque costs w_reg, w_lim (only for
421  // running models)
422  if (!is_terminal_) {
423  d->Lw.noalias() +=
424  wreg_ * d->r.segment(differential_->get_nr(), nw); // reg
425  d->Lw.noalias() += wlim_ * d->activation->Ar; // lim
426  d->Lww.diagonal().array() += Scalar(wreg_); // w reg
427  d->Lww.diagonal() += wlim_ * d->activation->Arr.diagonal(); // w lim
428  }
429  }
430  } // tau_plus_integration
431 }
432 
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);
437 }
438 
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);
443  if (data != NULL) {
444  return differential_->checkData(d->differential);
445  } else {
446  return false;
447  }
448 }
449 
450 template <typename Scalar>
451 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
452 IntegratedActionModelLPFTpl<Scalar>::get_differential() const {
453  return differential_;
454 }
455 
456 template <typename Scalar>
457 const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_dt() const {
458  return time_step_;
459 }
460 
461 template <typename Scalar>
462 const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_fc() const {
463  return fc_;
464 }
465 
466 template <typename Scalar>
467 void IntegratedActionModelLPFTpl<Scalar>::set_dt(const Scalar& dt) {
468  if (dt < 0.) {
469  throw_pretty("Invalid argument: "
470  << "dt has positive value");
471  }
472  time_step_ = dt;
473  time_step2_ = dt * dt;
474 }
475 
476 template <typename Scalar>
477 void IntegratedActionModelLPFTpl<Scalar>::set_fc(const Scalar& fc) {
478  // Set the cut-off frequency
479  if (fc <= 0.) {
480  throw_pretty("Invalid argument: "
481  << "fc must be positive");
482  } else {
483  fc_ = fc;
484  }
485 }
486 
487 template <typename Scalar>
488 void IntegratedActionModelLPFTpl<Scalar>::compute_alpha(const Scalar& fc) {
489  // Update alpha parameter
490  if (fc > 0 && time_step_ != 0) {
491  const Scalar& pi = 3.14159;
492  // Exponential Moving Average (EMA) (IIR filter) >> quite sharp
493  if (filter_ == 0) {
494  alpha_ = exp(-2. * pi * fc * time_step_);
495  }
496  // Using the formula "fc = 1/2*pi*RC" ??? >> less sharp
497  if (filter_ == 1) {
498  double omega = 1 / (2. * pi * time_step_ * fc);
499  alpha_ = omega / (omega + 1);
500  }
501  // Exact formula to get fc out of EMA's alpha >> inbetween sharp
502  if (filter_ == 2) {
503  double y = cos(2. * pi * time_step_ * fc);
504  alpha_ = 1 - (y - 1 + sqrt(y * y - 4 * y + 3));
505  }
506  } else {
507  alpha_ = 0;
508  }
509 }
510 
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();
515  if (nu_ != nu) {
516  nu_ = nu;
517  unone_ = VectorXs::Zero(nu_);
518  }
519  nr_ = model->get_nr() + 2 * nu;
520  state_ = boost::static_pointer_cast<StateLPF>(
521  model->get_state()); // cast StateAbstract from DAM as StateLPF for IAM
522  differential_ = model;
523  Base::set_u_lb(differential_->get_u_lb());
524  Base::set_u_ub(differential_->get_u_ub());
525 }
526 
527 template <typename Scalar>
528 void IntegratedActionModelLPFTpl<Scalar>::set_control_reg_cost(
529  const Scalar& weight, const VectorXs& ref) {
530  if (weight <= 0.) {
531  throw_pretty("cost weight is positive ");
532  }
533  wreg_ = weight;
534  wref_ = ref;
535 }
536 
537 template <typename Scalar>
538 void IntegratedActionModelLPFTpl<Scalar>::set_control_lim_cost(
539  const Scalar& weight) {
540  if (weight <= 0.) {
541  throw_pretty("cost weight is positive ");
542  }
543  wlim_ = weight;
544 }
545 
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,
550  const Scalar& tol) {
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_) + ")");
555  }
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()) + ")");
560  }
561 
562  // Static casting the data
563  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
564 
565  differential_->quasiStatic(d->differential, u, x, maxiter, tol);
566 }
567 
568 } // namespace sobec
Definition: contact-force.hxx:11