diff-action.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh, New York University,
5 // Max Planck Gesellschaft
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/core/numdiff/diff-action.hpp"
12 
13 namespace crocoddyl {
14 
15 template <typename Scalar>
16 DifferentialActionModelNumDiffTpl<Scalar>::DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model,
17  bool with_gauss_approx)
18  : Base(model->get_state(), model->get_nu(), model->get_nr()), model_(model) {
19  with_gauss_approx_ = with_gauss_approx;
20  disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
21  assert_pretty((!with_gauss_approx_ || nr_ > 1), "No Gauss approximation possible with nr = 1");
22 }
23 
24 template <typename Scalar>
25 DifferentialActionModelNumDiffTpl<Scalar>::~DifferentialActionModelNumDiffTpl() {}
26 
27 template <typename Scalar>
28 void DifferentialActionModelNumDiffTpl<Scalar>::calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
29  const Eigen::Ref<const VectorXs>& x,
30  const Eigen::Ref<const VectorXs>& u) {
31  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
32  throw_pretty("Invalid argument: "
33  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
34  }
35  if (static_cast<std::size_t>(u.size()) != nu_) {
36  throw_pretty("Invalid argument: "
37  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
38  }
39  DifferentialActionDataNumDiff* data_nd = static_cast<DifferentialActionDataNumDiff*>(data.get());
40  model_->calc(data_nd->data_0, x, u);
41  data->cost = data_nd->data_0->cost;
42  data->xout = data_nd->data_0->xout;
43 }
44 
45 template <typename Scalar>
46 void DifferentialActionModelNumDiffTpl<Scalar>::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
47  const Eigen::Ref<const VectorXs>& x,
48  const Eigen::Ref<const VectorXs>& u) {
49  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
50  throw_pretty("Invalid argument: "
51  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
52  }
53  if (static_cast<std::size_t>(u.size()) != nu_) {
54  throw_pretty("Invalid argument: "
55  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
56  }
57  boost::shared_ptr<DifferentialActionDataNumDiff> data_nd =
58  boost::static_pointer_cast<DifferentialActionDataNumDiff>(data);
59 
60  const VectorXs& xn0 = data_nd->data_0->xout;
61  const Scalar& c0 = data_nd->data_0->cost;
62  data->xout = data_nd->data_0->xout;
63  data->cost = data_nd->data_0->cost;
64 
65  assertStableStateFD(x);
66 
67  // Computing the d action(x,u) / dx
68  data_nd->dx.setZero();
69  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
70  data_nd->dx(ix) = disturbance_;
71  model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
72  model_->calc(data_nd->data_x[ix], data_nd->xp, u);
73 
74  const VectorXs& xn = data_nd->data_x[ix]->xout;
75  const Scalar& c = data_nd->data_x[ix]->cost;
76  data->Fx.col(ix) = (xn - xn0) / disturbance_;
77 
78  data->Lx(ix) = (c - c0) / disturbance_;
79  data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
80  data_nd->dx(ix) = 0.0;
81  }
82 
83  // Computing the d action(x,u) / du
84  data_nd->du.setZero();
85  for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
86  data_nd->du(iu) = disturbance_;
87  model_->calc(data_nd->data_u[iu], x, u + data_nd->du);
88 
89  const VectorXs& xn = data_nd->data_u[iu]->xout;
90  const Scalar& c = data_nd->data_u[iu]->cost;
91  data->Fu.col(iu) = (xn - xn0) / disturbance_;
92 
93  data->Lu(iu) = (c - c0) / disturbance_;
94  data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
95  data_nd->du(iu) = 0.0;
96  }
97 
98  if (with_gauss_approx_) {
99  data->Lxx = data_nd->Rx.transpose() * data_nd->Rx;
100  data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
101  data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
102  }
103 }
104 
105 template <typename Scalar>
106 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > DifferentialActionModelNumDiffTpl<Scalar>::createData() {
107  return boost::make_shared<DifferentialActionDataNumDiff>(this);
108 }
109 
110 template <typename Scalar>
111 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
112 DifferentialActionModelNumDiffTpl<Scalar>::get_model() const {
113  return model_;
114 }
115 
116 template <typename Scalar>
117 const Scalar& DifferentialActionModelNumDiffTpl<Scalar>::get_disturbance() const {
118  return disturbance_;
119 }
120 
121 template <typename Scalar>
122 void DifferentialActionModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
123  if (disturbance < 0.) {
124  throw_pretty("Invalid argument: "
125  << "Disturbance value is positive");
126  }
127  disturbance_ = disturbance;
128 }
129 
130 template <typename Scalar>
131 bool DifferentialActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
132  return with_gauss_approx_;
133 }
134 
135 template <typename Scalar>
136 void DifferentialActionModelNumDiffTpl<Scalar>::assertStableStateFD(const Eigen::Ref<const VectorXs>& ) {
137  // TODO(cmastalli): First we need to do it AMNumDiff and then to replicate it.
138 }
139 
140 } // namespace crocoddyl
Definition: action-base.hxx:11