GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/cost.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 96 108 88.9%
Branches: 67 144 46.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/numdiff/cost.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 648 CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(
16 const std::shared_ptr<Base>& model)
17 : Base(model->get_state(), model->get_activation(), model->get_nu()),
18 648 model_(model),
19
2/4
✓ Branch 7 taken 648 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 648 times.
✗ Branch 12 not taken.
 648 e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 648 void CostModelNumDiffTpl<Scalar>::calc(
23 const std::shared_ptr<CostDataAbstract>& data,
24 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
25 648 Data* d = static_cast<Data*>(data.get());
26 648 d->data_0->cost = Scalar(0.);
27 648 model_->calc(d->data_0, x, u);
28 648 d->cost = d->data_0->cost;
29 648 d->residual->r = d->data_0->residual->r;
30 648 }
31
32 template <typename Scalar>
33 324 void CostModelNumDiffTpl<Scalar>::calc(
34 const std::shared_ptr<CostDataAbstract>& data,
35 const Eigen::Ref<const VectorXs>& x) {
36 324 Data* d = static_cast<Data*>(data.get());
37 324 d->data_0->cost = Scalar(0.);
38 324 model_->calc(d->data_0, x);
39 324 d->cost = d->data_0->cost;
40 324 d->residual->r = d->data_0->residual->r;
41 324 }
42
43 template <typename Scalar>
44 324 void CostModelNumDiffTpl<Scalar>::calcDiff(
45 const std::shared_ptr<CostDataAbstract>& data,
46 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
47 324 Data* d = static_cast<Data*>(data.get());
48
49 324 const Scalar c0 = d->cost;
50 324 const VectorXs& r0 = d->residual->r;
51
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 324 if (get_with_gauss_approx()) {
52
1/2
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 model_->get_activation()->calc(d->data_0->activation, r0);
53
1/2
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 model_->get_activation()->calcDiff(d->data_0->activation, r0);
54 }
55 324 d->du.setZero();
56
57 324 assertStableStateFD(x);
58
59 // Computing the d cost(x,u) / dx
60
4/8
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 324 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 324 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 324 times.
✗ Branch 17 not taken.
 324 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
61 324 d->x_norm = d->dx.norm();
62 324 d->dx.setZero();
63 324 d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
64
2/2
✓ Branch 2 taken 12978 times.
✓ Branch 3 taken 324 times.
 13302 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
65 12978 d->dx(ix) = d->xh_jac;
66
2/4
✓ Branch 5 taken 12978 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12978 times.
✗ Branch 9 not taken.
 12978 model_->get_state()->integrate(x, d->dx, d->xp);
67 // call the update function on the pinocchio data
68
2/2
✓ Branch 1 taken 13104 times.
✓ Branch 2 taken 12978 times.
 26082 for (size_t i = 0; i < reevals_.size(); ++i) {
69
1/2
✓ Branch 3 taken 13104 times.
✗ Branch 4 not taken.
 13104 reevals_[i](d->xp, u);
70 }
71
1/2
✓ Branch 4 taken 12978 times.
✗ Branch 5 not taken.
 12978 model_->calc(d->data_x[ix], d->xp, u);
72 12978 d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
73
1/2
✓ Branch 1 taken 12978 times.
✗ Branch 2 not taken.
 12978 if (get_with_gauss_approx()) {
74
3/6
✓ Branch 5 taken 12978 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 12978 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12978 times.
✗ Branch 13 not taken.
 12978 d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
75 }
76 12978 d->dx(ix) = Scalar(0.);
77 }
78
79 // Computing the d cost(x,u) / du
80 324 d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
81
2/2
✓ Branch 2 taken 6489 times.
✓ Branch 3 taken 324 times.
 6813 for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
82 6489 d->du(iu) = d->uh_jac;
83
1/2
✓ Branch 2 taken 6489 times.
✗ Branch 3 not taken.
 6489 d->up = u + d->du;
84 // call the update function
85
2/2
✓ Branch 1 taken 6552 times.
✓ Branch 2 taken 6489 times.
 13041 for (std::size_t i = 0; i < reevals_.size(); ++i) {
86
1/2
✓ Branch 3 taken 6552 times.
✗ Branch 4 not taken.
 6552 reevals_[i](x, d->up);
87 }
88
1/2
✓ Branch 4 taken 6489 times.
✗ Branch 5 not taken.
 6489 model_->calc(d->data_u[iu], x, d->up);
89 6489 d->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
90
1/2
✓ Branch 1 taken 6489 times.
✗ Branch 2 not taken.
 6489 if (get_with_gauss_approx()) {
91
3/6
✓ Branch 5 taken 6489 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6489 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6489 times.
✗ Branch 13 not taken.
 6489 d->residual->Ru.col(iu) = (d->data_u[iu]->residual->r - r0) / d->uh_jac;
92 }
93 6489 d->du(iu) = Scalar(0.);
94 }
95
96
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 324 if (get_with_gauss_approx()) {
97
1/2
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
 324 const MatrixXs& Arr = d->data_0->activation->Arr;
98
4/8
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 324 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 324 times.
✗ Branch 13 not taken.
 324 d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
99
4/8
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 324 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 324 times.
✗ Branch 13 not taken.
 324 d->Lxu = d->residual->Rx.transpose() * Arr * d->residual->Ru;
100
4/8
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 324 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 324 times.
✗ Branch 13 not taken.
 324 d->Luu = d->residual->Ru.transpose() * Arr * d->residual->Ru;
101 324 } else {
102 ✗ d->Lxx.fill(Scalar(0.));
103 ✗ d->Lxu.fill(Scalar(0.));
104 ✗ d->Luu.fill(Scalar(0.));
105 }
106 324 }
107
108 template <typename Scalar>
109 324 void CostModelNumDiffTpl<Scalar>::calcDiff(
110 const std::shared_ptr<CostDataAbstract>& data,
111 const Eigen::Ref<const VectorXs>& x) {
112 324 Data* d = static_cast<Data*>(data.get());
113
114 324 const Scalar c0 = d->cost;
115 324 const VectorXs& r0 = d->residual->r;
116
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 324 if (get_with_gauss_approx()) {
117
1/2
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 model_->get_activation()->calc(d->data_0->activation, r0);
118
1/2
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 model_->get_activation()->calcDiff(d->data_0->activation, r0);
119 }
120 324 d->dx.setZero();
121
122 324 assertStableStateFD(x);
123
124 // Computing the d cost(x,u) / dx
125 324 d->xh_jac = e_jac_ * std::max(Scalar(1.), x.norm());
126
2/2
✓ Branch 2 taken 12978 times.
✓ Branch 3 taken 324 times.
 13302 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
127 12978 d->dx(ix) = d->xh_jac;
128
2/4
✓ Branch 5 taken 12978 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12978 times.
✗ Branch 9 not taken.
 12978 model_->get_state()->integrate(x, d->dx, d->xp);
129 // call the update function on the pinocchio data
130
2/2
✓ Branch 1 taken 13104 times.
✓ Branch 2 taken 12978 times.
 26082 for (size_t i = 0; i < reevals_.size(); ++i) {
131 13104 reevals_[i](d->xp, unone_);
132 }
133
1/2
✓ Branch 4 taken 12978 times.
✗ Branch 5 not taken.
 12978 model_->calc(d->data_x[ix], d->xp);
134 12978 d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
135
1/2
✓ Branch 1 taken 12978 times.
✗ Branch 2 not taken.
 12978 if (get_with_gauss_approx()) {
136
3/6
✓ Branch 5 taken 12978 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 12978 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12978 times.
✗ Branch 13 not taken.
 12978 d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
137 }
138 12978 d->dx(ix) = Scalar(0.);
139 }
140
141
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 324 if (get_with_gauss_approx()) {
142
1/2
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
 324 const MatrixXs& Arr = d->data_0->activation->Arr;
143
4/8
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 324 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 324 times.
✗ Branch 13 not taken.
 324 d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
144 324 } else {
145 ✗ d->Lxx.fill(Scalar(0.));
146 }
147 324 }
148
149 template <typename Scalar>
150 std::shared_ptr<CostDataAbstractTpl<Scalar> >
151 648 CostModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
152 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
153
1/2
✓ Branch 2 taken 648 times.
✗ Branch 3 not taken.
 648 data);
154 }
155
156 template <typename Scalar>
157 template <typename NewScalar>
158 ✗ CostModelNumDiffTpl<NewScalar> CostModelNumDiffTpl<Scalar>::cast() const {
159 typedef CostModelNumDiffTpl<NewScalar> ReturnType;
160 ✗ ReturnType res(model_->template cast<NewScalar>());
161 ✗ return res;
162 }
163
164 template <typename Scalar>
165 const std::shared_ptr<CostModelAbstractTpl<Scalar> >&
166 40878 CostModelNumDiffTpl<Scalar>::get_model() const {
167 40878 return model_;
168 }
169
170 template <typename Scalar>
171 639 const Scalar CostModelNumDiffTpl<Scalar>::get_disturbance() const {
172 639 return e_jac_;
173 }
174
175 template <typename Scalar>
176 ✗ void CostModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
177 ✗ if (disturbance < Scalar(0.)) {
178 ✗ throw_pretty("Invalid argument: " << "Disturbance constant is positive");
179 }
180 ✗ e_jac_ = disturbance;
181 }
182
183 template <typename Scalar>
184 34389 bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
185 34389 return activation_->get_nr() > 0;
186 }
187
188 template <typename Scalar>
189 324 void CostModelNumDiffTpl<Scalar>::set_reevals(
190 const std::vector<ReevaluationFunction>& reevals) {
191 324 reevals_ = reevals;
192 324 }
193
194 template <typename Scalar>
195 648 void CostModelNumDiffTpl<Scalar>::assertStableStateFD(
196 const Eigen::Ref<const VectorXs>& /*x*/) {
197 // do nothing in the general case
198 648 }
199
200 } // namespace crocoddyl
201