activation.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 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 ActivationModelNumDiffTpl<Scalar>::ActivationModelNumDiffTpl(boost::shared_ptr<Base> model)
16  : Base(model->get_nr()), model_(model) {
17  disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
18 }
19 
20 template <typename Scalar>
21 ActivationModelNumDiffTpl<Scalar>::~ActivationModelNumDiffTpl() {}
22 
23 template <typename Scalar>
24 void ActivationModelNumDiffTpl<Scalar>::calc(const boost::shared_ptr<ActivationDataAbstract>& data,
25  const Eigen::Ref<const VectorXs>& r) {
26  if (static_cast<std::size_t>(r.size()) != model_->get_nr()) {
27  throw_pretty("Invalid argument: "
28  << "r has wrong dimension (it should be " + std::to_string(model_->get_nr()) + ")");
29  }
30  boost::shared_ptr<ActivationDataNumDiffTpl<Scalar> > data_nd =
31  boost::static_pointer_cast<ActivationDataNumDiffTpl<Scalar> >(data);
32  model_->calc(data_nd->data_0, r);
33  data->a_value = data_nd->data_0->a_value;
34 }
35 
36 template <typename Scalar>
37 void ActivationModelNumDiffTpl<Scalar>::calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
38  const Eigen::Ref<const VectorXs>& r) {
39  if (static_cast<std::size_t>(r.size()) != model_->get_nr()) {
40  throw_pretty("Invalid argument: "
41  << "r has wrong dimension (it should be " + std::to_string(model_->get_nr()) + ")");
42  }
43  boost::shared_ptr<ActivationDataNumDiffTpl<Scalar> > data_nd =
44  boost::static_pointer_cast<ActivationDataNumDiffTpl<Scalar> >(data);
45 
46  const Scalar& a_value0 = data_nd->data_0->a_value;
47  data->a_value = data_nd->data_0->a_value;
48  const std::size_t& nr = model_->get_nr();
49 
50  // Computing the d activation(r) / dr
51  data_nd->rp = r;
52  for (unsigned int i_r = 0; i_r < nr; ++i_r) {
53  data_nd->rp(i_r) += disturbance_;
54  model_->calc(data_nd->data_rp[i_r], data_nd->rp);
55  data_nd->rp(i_r) -= disturbance_;
56  data->Ar(i_r) = (data_nd->data_rp[i_r]->a_value - a_value0) / disturbance_;
57  }
58 
59  // Computing the d^2 action(r) / dr^2
60  data->Arr.noalias() = data->Ar * data->Ar.transpose();
61 }
62 
63 template <typename Scalar>
64 boost::shared_ptr<ActivationDataAbstractTpl<Scalar> > ActivationModelNumDiffTpl<Scalar>::createData() {
65  return boost::make_shared<ActivationDataNumDiffTpl<Scalar> >(this);
66 }
67 
68 template <typename Scalar>
69 const boost::shared_ptr<ActivationModelAbstractTpl<Scalar> >& ActivationModelNumDiffTpl<Scalar>::get_model() const {
70  return model_;
71 }
72 
73 template <typename Scalar>
74 const Scalar& ActivationModelNumDiffTpl<Scalar>::get_disturbance() const {
75  return disturbance_;
76 }
77 
78 template <typename Scalar>
79 void ActivationModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
80  if (disturbance < 0.) {
81  throw_pretty("Invalid argument: "
82  << "Disturbance value is positive");
83  }
84  disturbance_ = disturbance;
85 }
86 
87 } // namespace crocoddyl
Definition: action-base.hxx:11