euler.hxx
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 <iostream>
10 
11 #include "crocoddyl/core/utils/exception.hpp"
12 #include "crocoddyl/core/integrator/euler.hpp"
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
18  boost::shared_ptr<DifferentialActionModelAbstract> model, const Scalar& time_step, const bool& with_cost_residual)
19  : Base(model->get_state(), model->get_nu(), model->get_nr()),
20  differential_(model),
21  time_step_(time_step),
22  time_step2_(time_step * time_step),
23  with_cost_residual_(with_cost_residual),
24  enable_integration_(true) {
25  Base::set_u_lb(differential_->get_u_lb());
26  Base::set_u_ub(differential_->get_u_ub());
27  if (time_step_ < 0.) {
28  time_step_ = 1e-3;
29  time_step2_ = time_step_ * time_step_;
30  std::cerr << "Warning: dt has positive value, set to 1e-3" << std::endl;
31  }
32  if (time_step == 0.) {
33  enable_integration_ = false;
34  }
35 }
36 
37 template <typename Scalar>
38 IntegratedActionModelEulerTpl<Scalar>::~IntegratedActionModelEulerTpl() {}
39 
40 template <typename Scalar>
41 void IntegratedActionModelEulerTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
42  const Eigen::Ref<const VectorXs>& x,
43  const Eigen::Ref<const VectorXs>& u) {
44  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
45  throw_pretty("Invalid argument: "
46  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
47  }
48  if (static_cast<std::size_t>(u.size()) != nu_) {
49  throw_pretty("Invalid argument: "
50  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
51  }
52 
53  // Static casting the data
54  boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
55 
56  // Computing the acceleration and cost
57  differential_->calc(d->differential, x, u);
58 
59  // Computing the next state (discrete time)
60  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
61  x.tail(differential_->get_state()->get_nv());
62  const VectorXs& a = d->differential->xout;
63  if (enable_integration_) {
64  d->dx << v * time_step_ + a * time_step2_, a * time_step_;
65  differential_->get_state()->integrate(x, d->dx, d->xnext);
66  d->cost = time_step_ * d->differential->cost;
67  } else {
68  d->dx.setZero();
69  d->xnext = x;
70  d->cost = d->differential->cost;
71  }
72 
73  // Updating the cost value
74  if (with_cost_residual_) {
75  d->r = d->differential->r;
76  }
77 }
78 
79 template <typename Scalar>
80 void IntegratedActionModelEulerTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
81  const Eigen::Ref<const VectorXs>& x,
82  const Eigen::Ref<const 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 " + std::to_string(state_->get_nx()) + ")");
86  }
87  if (static_cast<std::size_t>(u.size()) != nu_) {
88  throw_pretty("Invalid argument: "
89  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
90  }
91 
92  const std::size_t& nv = differential_->get_state()->get_nv();
93 
94  // Static casting the data
95  boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
96 
97  // Computing the derivatives for the time-continuous model (i.e. differential model)
98  differential_->calcDiff(d->differential, x, u);
99  differential_->get_state()->Jintegrate(x, d->dx, d->dxnext_dx, d->dxnext_ddx);
100 
101  d->Fx = d->dxnext_dx;
102  if (enable_integration_) {
103  const MatrixXs& da_dx = d->differential->Fx;
104  const MatrixXs& da_du = d->differential->Fu;
105  d->ddx_dx << da_dx * time_step_, da_dx;
106  d->ddx_du << da_du * time_step_, da_du;
107  for (std::size_t i = 0; i < nv; ++i) {
108  d->ddx_dx(i, i + nv) += 1.;
109  }
110  d->Fx.noalias() += time_step_ * (d->dxnext_ddx * d->ddx_dx);
111  d->Fu.noalias() = time_step_ * (d->dxnext_ddx * d->ddx_du);
112  d->Lx = time_step_ * d->differential->Lx;
113  d->Lu = time_step_ * d->differential->Lu;
114  d->Lxx = time_step_ * d->differential->Lxx;
115  d->Lxu = time_step_ * d->differential->Lxu;
116  d->Luu = time_step_ * d->differential->Luu;
117  } else {
118  d->Fu.setZero();
119  d->Lx = d->differential->Lx;
120  d->Lu = d->differential->Lu;
121  d->Lxx = d->differential->Lxx;
122  d->Lxu = d->differential->Lxu;
123  d->Luu = d->differential->Luu;
124  }
125 }
126 
127 template <typename Scalar>
128 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > IntegratedActionModelEulerTpl<Scalar>::createData() {
129  return boost::make_shared<IntegratedActionDataEuler>(this);
130 }
131 
132 template <typename Scalar>
133 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
134 IntegratedActionModelEulerTpl<Scalar>::get_differential() const {
135  return differential_;
136 }
137 
138 template <typename Scalar>
139 const Scalar& IntegratedActionModelEulerTpl<Scalar>::get_dt() const {
140  return time_step_;
141 }
142 
143 template <typename Scalar>
144 void IntegratedActionModelEulerTpl<Scalar>::set_dt(const Scalar& dt) {
145  if (dt < 0.) {
146  throw_pretty("Invalid argument: "
147  << "dt has positive value");
148  }
149  time_step_ = dt;
150  time_step2_ = dt * dt;
151 }
152 
153 template <typename Scalar>
154 void IntegratedActionModelEulerTpl<Scalar>::set_differential(
155  boost::shared_ptr<DifferentialActionModelAbstract> model) {
156  const std::size_t& nu = model->get_nu();
157  if (nu_ != nu) {
158  nu_ = nu;
159  unone_ = VectorXs::Zero(nu_);
160  }
161  nr_ = model->get_nr();
162  state_ = model->get_state();
163  differential_ = model;
164  Base::set_u_lb(differential_->get_u_lb());
165  Base::set_u_ub(differential_->get_u_ub());
166 }
167 
168 } // namespace crocoddyl
Definition: action-base.hxx:11