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/action.hpp"
12 
13 namespace crocoddyl {
14 
15 template <typename Scalar>
16 ActionModelNumDiffTpl<Scalar>::ActionModelNumDiffTpl(boost::shared_ptr<Base> model)
17  : Base(model->get_state(), model->get_nu(), model->get_nr()), model_(model) {
18  disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
19 }
20 
21 template <typename Scalar>
22 ActionModelNumDiffTpl<Scalar>::~ActionModelNumDiffTpl() {}
23 
24 template <typename Scalar>
25 void ActionModelNumDiffTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
26  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
27  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
28  throw_pretty("Invalid argument: "
29  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
30  }
31  if (static_cast<std::size_t>(u.size()) != nu_) {
32  throw_pretty("Invalid argument: "
33  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
34  }
35  boost::shared_ptr<ActionDataNumDiffTpl<Scalar> > data_nd =
36  boost::static_pointer_cast<ActionDataNumDiffTpl<Scalar> >(data);
37  model_->calc(data_nd->data_0, x, u);
38  data->cost = data_nd->data_0->cost;
39  data->xnext = data_nd->data_0->xnext;
40 }
41 
42 template <typename Scalar>
43 void ActionModelNumDiffTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
44  const Eigen::Ref<const VectorXs>& x,
45  const Eigen::Ref<const VectorXs>& u) {
46  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
47  throw_pretty("Invalid argument: "
48  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
49  }
50  if (static_cast<std::size_t>(u.size()) != nu_) {
51  throw_pretty("Invalid argument: "
52  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
53  }
54  boost::shared_ptr<ActionDataNumDiffTpl<Scalar> > data_nd =
55  boost::static_pointer_cast<ActionDataNumDiffTpl<Scalar> >(data);
56 
57  const VectorXs& xn0 = data_nd->data_0->xnext;
58  const Scalar& c0 = data_nd->data_0->cost;
59  data->xnext = data_nd->data_0->xnext;
60  data->cost = data_nd->data_0->cost;
61 
62  assertStableStateFD(x);
63 
64  // Computing the d action(x,u) / dx
65  data_nd->dx.setZero();
66  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
67  data_nd->dx(ix) = disturbance_;
68  model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
69  model_->calc(data_nd->data_x[ix], data_nd->xp, u);
70 
71  const VectorXs& xn = data_nd->data_x[ix]->xnext;
72  const Scalar& c = data_nd->data_x[ix]->cost;
73  model_->get_state()->diff(xn0, xn, data_nd->Fx.col(ix));
74 
75  data->Lx(ix) = (c - c0) / disturbance_;
76  if (get_with_gauss_approx() > 0) {
77  data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
78  }
79  data_nd->dx(ix) = 0.0;
80  }
81  data->Fx /= disturbance_;
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]->xnext;
90  const Scalar& c = data_nd->data_u[iu]->cost;
91  model_->get_state()->diff(xn0, xn, data_nd->Fu.col(iu));
92 
93  data->Lu(iu) = (c - c0) / disturbance_;
94  if (get_with_gauss_approx() > 0) {
95  data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
96  }
97  data_nd->du(iu) = 0.0;
98  }
99  data->Fu /= disturbance_;
100 
101  if (get_with_gauss_approx() > 0) {
102  data->Lxx = data_nd->Rx.transpose() * data_nd->Rx;
103  data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
104  data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
105  } else {
106  data->Lxx.setZero();
107  data->Lxu.setZero();
108  data->Luu.setZero();
109  }
110 }
111 
112 template <typename Scalar>
113 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelNumDiffTpl<Scalar>::createData() {
114  return boost::make_shared<ActionDataNumDiffTpl<Scalar> >(this);
115 }
116 
117 template <typename Scalar>
118 const boost::shared_ptr<ActionModelAbstractTpl<Scalar> >& ActionModelNumDiffTpl<Scalar>::get_model() const {
119  return model_;
120 }
121 
122 template <typename Scalar>
123 const Scalar& ActionModelNumDiffTpl<Scalar>::get_disturbance() const {
124  return disturbance_;
125 }
126 
127 template <typename Scalar>
128 void ActionModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
129  if (disturbance < 0.) {
130  throw_pretty("Invalid argument: "
131  << "Disturbance value is positive");
132  }
133  disturbance_ = disturbance;
134 }
135 
136 template <typename Scalar>
137 bool ActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
138  return model_->get_nr() > 0;
139 }
140 
141 template <typename Scalar>
142 void ActionModelNumDiffTpl<Scalar>::assertStableStateFD(const Eigen::Ref<const VectorXs>& ) {
143  // do nothing in the general case
144 }
145 
146 } // namespace crocoddyl
Definition: action-base.hxx:11