GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/residual.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 61 65 93.8%
Branches: 37 64 57.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/numdiff/residual.hpp"
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 648 ResidualModelNumDiffTpl<Scalar>::ResidualModelNumDiffTpl(
15 const std::shared_ptr<Base>& model)
16 : Base(model->get_state(), model->get_nr(), model->get_nu()),
17 648 model_(model),
18
1/2
✓ Branch 8 taken 324 times.
✗ Branch 9 not taken.
 648 e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
19
20 template <typename Scalar>
21 648 void ResidualModelNumDiffTpl<Scalar>::calc(
22 const std::shared_ptr<ResidualDataAbstract>& data,
23 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
24 648 Data* d = static_cast<Data*>(data.get());
25 648 model_->calc(d->data_0, x, u);
26 648 d->r = d->data_0->r;
27 }
28
29 template <typename Scalar>
30 324 void ResidualModelNumDiffTpl<Scalar>::calc(
31 const std::shared_ptr<ResidualDataAbstract>& data,
32 const Eigen::Ref<const VectorXs>& x) {
33 324 Data* d = static_cast<Data*>(data.get());
34 324 model_->calc(d->data_0, x);
35 324 d->r = d->data_0->r;
36 }
37
38 template <typename Scalar>
39 324 void ResidualModelNumDiffTpl<Scalar>::calcDiff(
40 const std::shared_ptr<ResidualDataAbstract>& data,
41 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
42 324 Data* d = static_cast<Data*>(data.get());
43
44 324 const VectorXs& r0 = d->r;
45 324 d->dx.setZero();
46 324 d->du.setZero();
47
48 324 assertStableStateFD(x);
49
50 // Computing the d residual(x,u) / dx
51
4/8
✓ Branch 6 taken 162 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 162 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 162 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 162 times.
✗ Branch 17 not taken.
 324 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
52 324 d->x_norm = d->dx.norm();
53 324 d->dx.setZero();
54 324 d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
55
2/2
✓ Branch 2 taken 7092 times.
✓ Branch 3 taken 162 times.
 14508 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
56 14184 d->dx(ix) = d->xh_jac;
57
2/4
✓ Branch 5 taken 7092 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7092 times.
✗ Branch 9 not taken.
 14184 model_->get_state()->integrate(x, d->dx, d->xp);
58 // call the update function
59
2/2
✓ Branch 1 taken 14184 times.
✓ Branch 2 taken 7092 times.
 42552 for (size_t i = 0; i < reevals_.size(); ++i) {
60
1/2
✓ Branch 3 taken 14184 times.
✗ Branch 4 not taken.
 28368 reevals_[i](d->xp, u);
61 }
62
1/2
✓ Branch 4 taken 7092 times.
✗ Branch 5 not taken.
 14184 model_->calc(d->data_x[ix], d->xp, u);
63
3/6
✓ Branch 4 taken 7092 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7092 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7092 times.
✗ Branch 11 not taken.
 14184 d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
64 14184 d->dx(ix) = Scalar(0.);
65 }
66
67 // Computing the d residual(x,u) / du
68 324 d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
69
2/2
✓ Branch 2 taken 3312 times.
✓ Branch 3 taken 162 times.
 6948 for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
70 6624 d->du(iu) = d->uh_jac;
71
1/2
✓ Branch 2 taken 3312 times.
✗ Branch 3 not taken.
 6624 d->up = u + d->du;
72 // call the update function
73
2/2
✓ Branch 1 taken 6624 times.
✓ Branch 2 taken 3312 times.
 19872 for (std::size_t i = 0; i < reevals_.size(); ++i) {
74
1/2
✓ Branch 3 taken 6624 times.
✗ Branch 4 not taken.
 13248 reevals_[i](x, d->up);
75 }
76
1/2
✓ Branch 4 taken 3312 times.
✗ Branch 5 not taken.
 6624 model_->calc(d->data_u[iu], x, d->up);
77
3/6
✓ Branch 4 taken 3312 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3312 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3312 times.
✗ Branch 11 not taken.
 6624 d->Ru.col(iu) = (d->data_u[iu]->r - r0) / d->uh_jac;
78 6624 d->du(iu) = Scalar(0.);
79 }
80 }
81
82 template <typename Scalar>
83 324 void ResidualModelNumDiffTpl<Scalar>::calcDiff(
84 const std::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) {
86 324 Data* d = static_cast<Data*>(data.get());
87
88 324 const VectorXs& r0 = d->r;
89 324 assertStableStateFD(x);
90
91 // Computing the d residual(x,u) / dx
92 324 d->dx.setZero();
93
2/2
✓ Branch 2 taken 7092 times.
✓ Branch 3 taken 162 times.
 14508 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
94 14184 d->dx(ix) = d->xh_jac;
95
2/4
✓ Branch 5 taken 7092 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7092 times.
✗ Branch 9 not taken.
 14184 model_->get_state()->integrate(x, d->dx, d->xp);
96 // call the update function
97
2/2
✓ Branch 1 taken 14184 times.
✓ Branch 2 taken 7092 times.
 42552 for (size_t i = 0; i < reevals_.size(); ++i) {
98 28368 reevals_[i](d->xp, unone_);
99 }
100
1/2
✓ Branch 4 taken 7092 times.
✗ Branch 5 not taken.
 14184 model_->calc(d->data_x[ix], d->xp);
101
3/6
✓ Branch 4 taken 7092 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7092 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7092 times.
✗ Branch 11 not taken.
 14184 d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
102 14184 d->dx(ix) = Scalar(0.);
103 }
104 }
105
106 template <typename Scalar>
107 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
108 648 ResidualModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
109 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
110
1/2
✓ Branch 2 taken 324 times.
✗ Branch 3 not taken.
 648 data);
111 }
112
113 template <typename Scalar>
114 template <typename NewScalar>
115 ✗ ResidualModelNumDiffTpl<NewScalar> ResidualModelNumDiffTpl<Scalar>::cast()
116 const {
117 typedef ResidualModelNumDiffTpl<NewScalar> ReturnType;
118 ✗ ReturnType res(model_->template cast<NewScalar>());
119 ✗ return res;
120 }
121
122 template <typename Scalar>
123 const std::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
124 43560 ResidualModelNumDiffTpl<Scalar>::get_model() const {
125 43560 return model_;
126 }
127
128 template <typename Scalar>
129 162 const Scalar ResidualModelNumDiffTpl<Scalar>::get_disturbance() const {
130 162 return e_jac_;
131 }
132
133 template <typename Scalar>
134 void ResidualModelNumDiffTpl<Scalar>::set_disturbance(
135 const Scalar disturbance) {
136 if (disturbance < Scalar(0.)) {
137 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
138 }
139 e_jac_ = disturbance;
140 }
141
142 template <typename Scalar>
143 324 void ResidualModelNumDiffTpl<Scalar>::set_reevals(
144 const std::vector<ReevaluationFunction>& reevals) {
145 324 reevals_ = reevals;
146 324 }
147
148 template <typename Scalar>
149 648 void ResidualModelNumDiffTpl<Scalar>::assertStableStateFD(
150 const Eigen::Ref<const VectorXs>& /*x*/) {
151 // do nothing in the general case
152 }
153
154 } // namespace crocoddyl
155