GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/constraint.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 97 104 93.3%
Branches: 54 192 28.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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/constraint.hpp"
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 140 ConstraintModelNumDiffTpl<Scalar>::ConstraintModelNumDiffTpl(
15 const std::shared_ptr<Base>& model)
16 : Base(model->get_state(), model->get_nu(), model->get_ng(),
17 model->get_nh()),
18 140 model_(model),
19
1/2
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
 140 e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 280 ConstraintModelNumDiffTpl<Scalar>::~ConstraintModelNumDiffTpl() {}
23
24 template <typename Scalar>
25 140 void ConstraintModelNumDiffTpl<Scalar>::calc(
26 const std::shared_ptr<ConstraintDataAbstract>& data,
27 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 140 times.
 140 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
29 ✗ throw_pretty(
30 "Invalid argument: " << "x has wrong dimension (it should be " +
31 std::to_string(state_->get_nx()) + ")");
32 }
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 140 times.
 140 if (static_cast<std::size_t>(u.size()) != nu_) {
34 ✗ throw_pretty(
35 "Invalid argument: " << "u has wrong dimension (it should be " +
36 std::to_string(nu_) + ")");
37 }
38
39 140 Data* d = static_cast<Data*>(data.get());
40 140 d->data_0->g.setZero();
41 140 d->data_0->h.setZero();
42 140 model_->calc(d->data_0, x, u);
43 140 d->g = d->data_0->g;
44 140 d->h = d->data_0->h;
45 140 }
46
47 template <typename Scalar>
48 70 void ConstraintModelNumDiffTpl<Scalar>::calc(
49 const std::shared_ptr<ConstraintDataAbstract>& data,
50 const Eigen::Ref<const VectorXs>& x) {
51
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 70 times.
 70 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
52 ✗ throw_pretty(
53 "Invalid argument: " << "x has wrong dimension (it should be " +
54 std::to_string(state_->get_nx()) + ")");
55 }
56 70 Data* d = static_cast<Data*>(data.get());
57
58 70 d->data_0->g.setZero();
59 70 d->data_0->h.setZero();
60 70 model_->calc(d->data_0, x);
61 70 d->g = d->data_0->g;
62 70 d->h = d->data_0->h;
63 70 }
64
65 template <typename Scalar>
66 70 void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
67 const std::shared_ptr<ConstraintDataAbstract>& data,
68 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
69
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 70 times.
 70 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
70 ✗ throw_pretty(
71 "Invalid argument: " << "x has wrong dimension (it should be " +
72 std::to_string(state_->get_nx()) + ")");
73 }
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 70 times.
 70 if (static_cast<std::size_t>(u.size()) != nu_) {
75 ✗ throw_pretty(
76 "Invalid argument: " << "u has wrong dimension (it should be " +
77 std::to_string(nu_) + ")");
78 }
79 70 Data* d = static_cast<Data*>(data.get());
80
81 70 const VectorXs& g0 = d->g;
82 70 const VectorXs& h0 = d->h;
83 70 const std::size_t ndx = model_->get_state()->get_ndx();
84 70 const std::size_t nu = model_->get_nu();
85 70 const std::size_t ng = model_->get_ng();
86 70 const std::size_t nh = model_->get_nh();
87 70 d->Gx.resize(ng, ndx);
88 70 d->Gu.resize(ng, nu);
89 70 d->Hx.resize(nh, ndx);
90 70 d->Hu.resize(nh, nu);
91 70 d->du.setZero();
92
93 70 assertStableStateFD(x);
94
95 // Computing the d constraint(x,u) / dx
96
3/6
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
97 70 d->x_norm = d->dx.norm();
98 70 d->dx.setZero();
99 70 d->xh_jac = e_jac_ * std::max(1., d->x_norm);
100
2/2
✓ Branch 2 taken 2856 times.
✓ Branch 3 taken 70 times.
 2926 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
101 2856 d->dx(ix) = d->xh_jac;
102
2/4
✓ Branch 5 taken 2856 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2856 times.
✗ Branch 9 not taken.
 2856 model_->get_state()->integrate(x, d->dx, d->xp);
103 // call the update function
104
2/2
✓ Branch 1 taken 2856 times.
✓ Branch 2 taken 2856 times.
 5712 for (size_t i = 0; i < reevals_.size(); ++i) {
105
1/2
✓ Branch 3 taken 2856 times.
✗ Branch 4 not taken.
 2856 reevals_[i](d->xp, u);
106 }
107
1/2
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
 2856 model_->calc(d->data_x[ix], d->xp, u);
108
3/6
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2856 times.
✗ Branch 11 not taken.
 2856 d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
109
3/6
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2856 times.
✗ Branch 11 not taken.
 2856 d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
110 2856 d->dx(ix) = 0.;
111 }
112
113 // Computing the d constraint(x,u) / du
114 70 d->uh_jac = e_jac_ * std::max(1., u.norm());
115
2/2
✓ Branch 2 taken 1428 times.
✓ Branch 3 taken 70 times.
 1498 for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
116 1428 d->du(iu) = d->uh_jac;
117
1/2
✓ Branch 2 taken 1428 times.
✗ Branch 3 not taken.
 1428 d->up = u + d->du;
118 // call the update function
119
2/2
✓ Branch 1 taken 1428 times.
✓ Branch 2 taken 1428 times.
 2856 for (std::size_t i = 0; i < reevals_.size(); ++i) {
120
1/2
✓ Branch 3 taken 1428 times.
✗ Branch 4 not taken.
 1428 reevals_[i](x, d->up);
121 }
122
1/2
✓ Branch 4 taken 1428 times.
✗ Branch 5 not taken.
 1428 model_->calc(d->data_u[iu], x, d->up);
123
3/6
✓ Branch 4 taken 1428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1428 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1428 times.
✗ Branch 11 not taken.
 1428 d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
124
3/6
✓ Branch 4 taken 1428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1428 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1428 times.
✗ Branch 11 not taken.
 1428 d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
125 1428 d->du(iu) = 0.;
126 }
127 70 }
128
129 template <typename Scalar>
130 70 void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
131 const std::shared_ptr<ConstraintDataAbstract>& data,
132 const Eigen::Ref<const VectorXs>& x) {
133
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 70 times.
 70 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
134 ✗ throw_pretty(
135 "Invalid argument: " << "x has wrong dimension (it should be " +
136 std::to_string(state_->get_nx()) + ")");
137 }
138 70 Data* d = static_cast<Data*>(data.get());
139
140 70 const VectorXs& g0 = d->g;
141 70 const VectorXs& h0 = d->h;
142 70 const std::size_t ndx = model_->get_state()->get_ndx();
143 70 d->Gx.resize(model_->get_ng(), ndx);
144 70 d->Hx.resize(model_->get_nh(), ndx);
145
146 70 assertStableStateFD(x);
147
148 // Computing the d constraint(x) / dx
149
3/6
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
150 70 d->x_norm = d->dx.norm();
151 70 d->dx.setZero();
152 70 d->xh_jac = e_jac_ * std::max(1., d->x_norm);
153
2/2
✓ Branch 2 taken 2856 times.
✓ Branch 3 taken 70 times.
 2926 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
154 // x + dx
155 2856 d->dx(ix) = d->xh_jac;
156
2/4
✓ Branch 5 taken 2856 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2856 times.
✗ Branch 9 not taken.
 2856 model_->get_state()->integrate(x, d->dx, d->xp);
157 // call the update function
158
2/2
✓ Branch 1 taken 2856 times.
✓ Branch 2 taken 2856 times.
 5712 for (size_t i = 0; i < reevals_.size(); ++i) {
159 2856 reevals_[i](d->xp, unone_);
160 }
161
1/2
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
 2856 model_->calc(d->data_x[ix], d->xp);
162
3/6
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2856 times.
✗ Branch 11 not taken.
 2856 d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
163
3/6
✓ Branch 4 taken 2856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2856 times.
✗ Branch 11 not taken.
 2856 d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
164 2856 d->dx(ix) = 0.;
165 }
166 70 }
167
168 template <typename Scalar>
169 std::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
170 140 ConstraintModelNumDiffTpl<Scalar>::createData(
171 DataCollectorAbstract* const data) {
172 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
173
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 data);
174 }
175
176 template <typename Scalar>
177 const std::shared_ptr<ConstraintModelAbstractTpl<Scalar> >&
178 8988 ConstraintModelNumDiffTpl<Scalar>::get_model() const {
179 8988 return model_;
180 }
181
182 template <typename Scalar>
183 70 const Scalar ConstraintModelNumDiffTpl<Scalar>::get_disturbance() const {
184 70 return e_jac_;
185 }
186
187 template <typename Scalar>
188 void ConstraintModelNumDiffTpl<Scalar>::set_disturbance(
189 const Scalar disturbance) {
190 if (disturbance < 0.) {
191 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
192 }
193 e_jac_ = disturbance;
194 }
195
196 template <typename Scalar>
197 70 void ConstraintModelNumDiffTpl<Scalar>::set_reevals(
198 const std::vector<ReevaluationFunction>& reevals) {
199 70 reevals_ = reevals;
200 70 }
201
202 template <typename Scalar>
203 140 void ConstraintModelNumDiffTpl<Scalar>::assertStableStateFD(
204 const Eigen::Ref<const VectorXs>& /*x*/) {
205 // do nothing in the general case
206 140 }
207
208 } // namespace crocoddyl
209