cost.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/numdiff/cost.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(const boost::shared_ptr<Base>& model)
16  : Base(model->get_state(), model->get_activation(), model->get_nu()), model_(model) {
17  disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
18 }
19 
20 template <typename Scalar>
21 CostModelNumDiffTpl<Scalar>::~CostModelNumDiffTpl() {}
22 
23 template <typename Scalar>
24 void CostModelNumDiffTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
25  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
26  boost::shared_ptr<CostDataNumDiffTpl<Scalar> > data_nd =
27  boost::static_pointer_cast<CostDataNumDiffTpl<Scalar> >(data);
28  data_nd->data_0->cost = 0.0;
29  model_->calc(data_nd->data_0, x, u);
30  data_nd->cost = data_nd->data_0->cost;
31  data_nd->r = data_nd->data_0->r;
32 }
33 
34 template <typename Scalar>
35 void CostModelNumDiffTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
36  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
37  boost::shared_ptr<CostDataNumDiffTpl<Scalar> > data_nd =
38  boost::static_pointer_cast<CostDataNumDiffTpl<Scalar> >(data);
39 
40  const Scalar& c0 = data_nd->cost;
41  const VectorXs& r0 = data_nd->r;
42  if (get_with_gauss_approx()) {
43  model_->get_activation()->calc(data_nd->data_0->activation, r0);
44  model_->get_activation()->calcDiff(data_nd->data_0->activation, r0);
45  }
46  assertStableStateFD(x);
47 
48  // Computing the d cost(x,u) / dx
49  data_nd->dx.setZero();
50  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
51  // x + dx
52  data_nd->dx(ix) = disturbance_;
53  model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
54  // call the update function on the pinocchio data
55  for (size_t i = 0; i < reevals_.size(); ++i) {
56  reevals_[i](data_nd->xp);
57  }
58  // cost(x+dx, u)
59  model_->calc(data_nd->data_x[ix], data_nd->xp, u);
60  // Lx
61  data_nd->Lx(ix) = (data_nd->data_x[ix]->cost - c0) / disturbance_;
62  // Check if we need to/can compute the Gauss approximation of the Hessian.
63  if (get_with_gauss_approx()) {
64  data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - r0) / disturbance_;
65  }
66  data_nd->dx(ix) = 0.0;
67  }
68 
69  // Computing the d cost(x,u) / du
70  data_nd->du.setZero();
71  // call the update function on the pinocchio data
72  for (size_t i = 0; i < reevals_.size(); ++i) {
73  reevals_[i](x);
74  }
75  for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
76  // up = u + du
77  data_nd->du(iu) = disturbance_;
78  data_nd->up = u + data_nd->du;
79  // cost(x, u+du)
80  model_->calc(data_nd->data_u[iu], x, data_nd->up);
81  // Lu
82  data_nd->Lu(iu) = (data_nd->data_u[iu]->cost - c0) / disturbance_;
83  // Check if we need to/can compute the Gauss approximation of the Hessian.
84  if (get_with_gauss_approx()) {
85  data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - r0) / disturbance_;
86  }
87  data_nd->du(iu) = 0.0;
88  }
89 
90  if (get_with_gauss_approx()) {
91  const MatrixXs& Arr = data_nd->data_0->activation->Arr;
92  data_nd->Lxx = data_nd->Rx.transpose() * Arr * data_nd->Rx;
93  data_nd->Lxu = data_nd->Rx.transpose() * Arr * data_nd->Ru;
94  data_nd->Luu = data_nd->Ru.transpose() * Arr * data_nd->Ru;
95  } else {
96  data_nd->Lxx.fill(0.0);
97  data_nd->Lxu.fill(0.0);
98  data_nd->Luu.fill(0.0);
99  }
100 }
101 
102 template <typename Scalar>
103 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelNumDiffTpl<Scalar>::createData(
104  DataCollectorAbstract* const data) {
105  return boost::make_shared<CostDataNumDiffTpl<Scalar> >(this, data);
106 }
107 
108 template <typename Scalar>
109 const boost::shared_ptr<CostModelAbstractTpl<Scalar> >& CostModelNumDiffTpl<Scalar>::get_model() const {
110  return model_;
111 }
112 
113 template <typename Scalar>
114 const Scalar& CostModelNumDiffTpl<Scalar>::get_disturbance() const {
115  return disturbance_;
116 }
117 
118 template <typename Scalar>
119 void CostModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
120  disturbance_ = disturbance;
121 }
122 
123 template <typename Scalar>
124 bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
125  return activation_->get_nr() > 0;
126 }
127 
128 template <typename Scalar>
129 void CostModelNumDiffTpl<Scalar>::set_reevals(const std::vector<ReevaluationFunction>& reevals) {
130  reevals_ = reevals;
131 }
132 
133 template <typename Scalar>
134 void CostModelNumDiffTpl<Scalar>::assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/) {
135  // do nothing in the general case
136 }
137 
138 } // namespace crocoddyl
Definition: action-base.hxx:11