10 #include "crocoddyl/core/utils/exception.hpp" 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());
20 template <
typename Scalar>
21 ActivationModelNumDiffTpl<Scalar>::~ActivationModelNumDiffTpl() {}
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()) +
")");
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;
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()) +
")");
43 boost::shared_ptr<ActivationDataNumDiffTpl<Scalar> > data_nd =
44 boost::static_pointer_cast<ActivationDataNumDiffTpl<Scalar> >(data);
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();
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_;
60 data->Arr.noalias() = data->Ar * data->Ar.transpose();
63 template <
typename Scalar>
64 boost::shared_ptr<ActivationDataAbstractTpl<Scalar> > ActivationModelNumDiffTpl<Scalar>::createData() {
65 return boost::make_shared<ActivationDataNumDiffTpl<Scalar> >(
this);
68 template <
typename Scalar>
69 const boost::shared_ptr<ActivationModelAbstractTpl<Scalar> >& ActivationModelNumDiffTpl<Scalar>::get_model()
const {
73 template <
typename Scalar>
74 const Scalar& ActivationModelNumDiffTpl<Scalar>::get_disturbance()
const {
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");
84 disturbance_ = disturbance;
Definition: action-base.hxx:11