GCC Code Coverage Report

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