GCC Code Coverage Report

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