actuation.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/core/numdiff/actuation.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(boost::shared_ptr<Base> model)
16  : Base(model->get_state(), model->get_nu()), model_(model) {
17  disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
18 }
19 
20 template <typename Scalar>
21 ActuationModelNumDiffTpl<Scalar>::~ActuationModelNumDiffTpl() {}
22 
23 template <typename Scalar>
24 void ActuationModelNumDiffTpl<Scalar>::calc(const boost::shared_ptr<ActuationDataAbstract>& data,
25  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
26  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
27  throw_pretty("Invalid argument: "
28  << "x has wrong dimension (it should be " + std::to_string(model_->get_state()->get_nx()) + ")");
29  }
30  if (static_cast<std::size_t>(u.size()) != nu_) {
31  throw_pretty("Invalid argument: "
32  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
33  }
34  boost::shared_ptr<ActuationDataNumDiffTpl<Scalar> > data_nd =
35  boost::static_pointer_cast<ActuationDataNumDiffTpl<Scalar> >(data);
36  model_->calc(data_nd->data_0, x, u);
37  data->tau = data_nd->data_0->tau;
38 }
39 
40 template <typename Scalar>
41 void ActuationModelNumDiffTpl<Scalar>::calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
42  const Eigen::Ref<const VectorXs>& x,
43  const Eigen::Ref<const VectorXs>& u) {
44  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
45  throw_pretty("Invalid argument: "
46  << "x has wrong dimension (it should be " + std::to_string(model_->get_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  boost::shared_ptr<ActuationDataNumDiffTpl<Scalar> > data_nd =
53  boost::static_pointer_cast<ActuationDataNumDiffTpl<Scalar> >(data);
54 
55  const VectorXs& tau0 = data_nd->data_0->tau;
56 
57  // Computing the d Actuation(x,u) / dx
58  data_nd->dx.setZero();
59  for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
60  data_nd->dx(ix) = disturbance_;
61  model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
62  model_->calc(data_nd->data_x[ix], data_nd->xp, u);
63  data_nd->dtau_dx.col(ix) = (data_nd->data_x[ix]->tau - tau0) / disturbance_;
64  data_nd->dx(ix) = 0.0;
65  }
66 
67  // Computing the d Actuation(x,u) / du
68  data_nd->du.setZero();
69  for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
70  data_nd->du(iu) = disturbance_;
71  model_->calc(data_nd->data_u[iu], x, u + data_nd->du);
72  data_nd->dtau_du.col(iu) = (data_nd->data_u[iu]->tau - tau0) / disturbance_;
73  data_nd->du(iu) = 0.0;
74  }
75 }
76 
77 template <typename Scalar>
78 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > ActuationModelNumDiffTpl<Scalar>::createData() {
79  return boost::make_shared<ActuationDataNumDiffTpl<Scalar> >(this);
80 }
81 
82 template <typename Scalar>
83 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >& ActuationModelNumDiffTpl<Scalar>::get_model() const {
84  return model_;
85 }
86 
87 template <typename Scalar>
88 const Scalar& ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
89  return disturbance_;
90 }
91 
92 template <typename Scalar>
93 void ActuationModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
94  if (disturbance < 0.) {
95  throw_pretty("Invalid argument: "
96  << "Disturbance value is positive");
97  }
98  disturbance_ = disturbance;
99 }
100 
101 } // namespace crocoddyl
Definition: action-base.hxx:11