GCC Code Coverage Report

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