| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // New York University, Max Planck Gesellschaft | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | namespace crocoddyl { | ||
| 11 | |||
| 12 | template <typename Scalar> | ||
| 13 | ✗ | ActivationModelNumDiffTpl<Scalar>::ActivationModelNumDiffTpl( | |
| 14 | std::shared_ptr<Base> model) | ||
| 15 | : Base(model->get_nr()), | ||
| 16 | ✗ | model_(model), | |
| 17 | ✗ | e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {} | |
| 18 | |||
| 19 | template <typename Scalar> | ||
| 20 | ✗ | ActivationModelNumDiffTpl<Scalar>::~ActivationModelNumDiffTpl() {} | |
| 21 | |||
| 22 | template <typename Scalar> | ||
| 23 | ✗ | void ActivationModelNumDiffTpl<Scalar>::calc( | |
| 24 | const std::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( | |
| 28 | "Invalid argument: " << "r has wrong dimension (it should be " + | ||
| 29 | std::to_string(model_->get_nr()) + ")"); | ||
| 30 | } | ||
| 31 | ✗ | std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(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( | |
| 38 | const std::shared_ptr<ActivationDataAbstract>& data, | ||
| 39 | const Eigen::Ref<const VectorXs>& r) { | ||
| 40 | ✗ | if (static_cast<std::size_t>(r.size()) != model_->get_nr()) { | |
| 41 | ✗ | throw_pretty( | |
| 42 | "Invalid argument: " << "r has wrong dimension (it should be " + | ||
| 43 | std::to_string(model_->get_nr()) + ")"); | ||
| 44 | } | ||
| 45 | ✗ | std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data); | |
| 46 | |||
| 47 | ✗ | const Scalar a_value0 = data_nd->data_0->a_value; | |
| 48 | ✗ | data->a_value = data_nd->data_0->a_value; | |
| 49 | ✗ | const std::size_t nr = model_->get_nr(); | |
| 50 | |||
| 51 | // Computing the d activation(r) / dr | ||
| 52 | ✗ | const Scalar rh_jac = e_jac_ * std::max(Scalar(1.), r.norm()); | |
| 53 | ✗ | data_nd->rp = r; | |
| 54 | ✗ | for (unsigned int i_r = 0; i_r < nr; ++i_r) { | |
| 55 | ✗ | data_nd->rp(i_r) += rh_jac; | |
| 56 | ✗ | model_->calc(data_nd->data_rp[i_r], data_nd->rp); | |
| 57 | ✗ | data_nd->rp(i_r) -= rh_jac; | |
| 58 | ✗ | data->Ar(i_r) = (data_nd->data_rp[i_r]->a_value - a_value0) / rh_jac; | |
| 59 | } | ||
| 60 | |||
| 61 | // Computing the d^2 action(r) / dr^2 | ||
| 62 | ✗ | data_nd->Arr_.noalias() = data->Ar * data->Ar.transpose(); | |
| 63 | ✗ | data->Arr.diagonal() = data_nd->Arr_.diagonal(); | |
| 64 | ✗ | } | |
| 65 | |||
| 66 | template <typename Scalar> | ||
| 67 | std::shared_ptr<ActivationDataAbstractTpl<Scalar> > | ||
| 68 | ✗ | ActivationModelNumDiffTpl<Scalar>::createData() { | |
| 69 | ✗ | return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); | |
| 70 | } | ||
| 71 | |||
| 72 | template <typename Scalar> | ||
| 73 | template <typename NewScalar> | ||
| 74 | ✗ | ActivationModelNumDiffTpl<NewScalar> ActivationModelNumDiffTpl<Scalar>::cast() | |
| 75 | const { | ||
| 76 | typedef ActivationModelNumDiffTpl<NewScalar> ReturnType; | ||
| 77 | ✗ | ReturnType res(model_->template cast<NewScalar>()); | |
| 78 | ✗ | return res; | |
| 79 | } | ||
| 80 | |||
| 81 | template <typename Scalar> | ||
| 82 | const std::shared_ptr<ActivationModelAbstractTpl<Scalar> >& | ||
| 83 | ✗ | ActivationModelNumDiffTpl<Scalar>::get_model() const { | |
| 84 | ✗ | return model_; | |
| 85 | } | ||
| 86 | |||
| 87 | template <typename Scalar> | ||
| 88 | ✗ | const Scalar ActivationModelNumDiffTpl<Scalar>::get_disturbance() const { | |
| 89 | ✗ | return e_jac_; | |
| 90 | } | ||
| 91 | |||
| 92 | template <typename Scalar> | ||
| 93 | ✗ | void ActivationModelNumDiffTpl<Scalar>::set_disturbance( | |
| 94 | const Scalar disturbance) { | ||
| 95 | ✗ | if (disturbance < Scalar(0.)) { | |
| 96 | ✗ | throw_pretty("Invalid argument: " << "Disturbance constant is positive"); | |
| 97 | } | ||
| 98 | ✗ | e_jac_ = disturbance; | |
| 99 | ✗ | } | |
| 100 | |||
| 101 | } // namespace crocoddyl | ||
| 102 |