GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/activation.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 35 41 85.4%
Branches: 23 92 25.0%
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 19 ActivationModelNumDiffTpl<Scalar>::ActivationModelNumDiffTpl(
14 std::shared_ptr<Base> model)
15 : Base(model->get_nr()),
16 19 model_(model),
17 19 e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
18
19 template <typename Scalar>
20 42 ActivationModelNumDiffTpl<Scalar>::~ActivationModelNumDiffTpl() {}
21
22 template <typename Scalar>
23 9 void ActivationModelNumDiffTpl<Scalar>::calc(
24 const std::shared_ptr<ActivationDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>& r) {
26
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 9 times.
 9 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 9 std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
32
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 model_->calc(data_nd->data_0, r);
33 9 data->a_value = data_nd->data_0->a_value;
34 9 }
35
36 template <typename Scalar>
37 9 void ActivationModelNumDiffTpl<Scalar>::calcDiff(
38 const std::shared_ptr<ActivationDataAbstract>& data,
39 const Eigen::Ref<const VectorXs>& r) {
40
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 9 times.
 9 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 9 std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
46
47 9 const Scalar a_value0 = data_nd->data_0->a_value;
48 9 data->a_value = data_nd->data_0->a_value;
49
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 const std::size_t nr = model_->get_nr();
50
51 // Computing the d activation(r) / dr
52
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 const Scalar rh_jac = e_jac_ * std::max(Scalar(1.), r.norm());
53
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 data_nd->rp = r;
54
2/2
✓ Branch 0 taken 45 times.
✓ Branch 1 taken 9 times.
 54 for (unsigned int i_r = 0; i_r < nr; ++i_r) {
55
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 data_nd->rp(i_r) += rh_jac;
56
2/4
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 model_->calc(data_nd->data_rp[i_r], data_nd->rp);
57
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 data_nd->rp(i_r) -= rh_jac;
58
1/2
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 45 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
4/8
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 9 data_nd->Arr_.noalias() = data->Ar * data->Ar.transpose();
63
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 data->Arr.diagonal() = data_nd->Arr_.diagonal();
64 9 }
65
66 template <typename Scalar>
67 std::shared_ptr<ActivationDataAbstractTpl<Scalar> >
68 11 ActivationModelNumDiffTpl<Scalar>::createData() {
69
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
70 }
71
72 template <typename Scalar>
73 template <typename NewScalar>
74 9 ActivationModelNumDiffTpl<NewScalar> ActivationModelNumDiffTpl<Scalar>::cast()
75 const {
76 typedef ActivationModelNumDiffTpl<NewScalar> ReturnType;
77
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 ReturnType res(model_->template cast<NewScalar>());
78 9 return res;
79 }
80
81 template <typename Scalar>
82 const std::shared_ptr<ActivationModelAbstractTpl<Scalar> >&
83 137 ActivationModelNumDiffTpl<Scalar>::get_model() const {
84 137 return model_;
85 }
86
87 template <typename Scalar>
88 18 const Scalar ActivationModelNumDiffTpl<Scalar>::get_disturbance() const {
89 18 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