GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/constraint.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 100 0.0%
Branches: 0 206 0.0%
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 ✗ 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 ✗ model_(model),
19 ✗ e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 ✗ 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 ✗ 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 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
37 ✗ d->data_0->g.setZero();
38 ✗ d->data_0->h.setZero();
39 ✗ model_->calc(d->data_0, x, u);
40 ✗ d->g = d->data_0->g;
41 ✗ d->h = d->data_0->h;
42 }
43
44 template <typename Scalar>
45 ✗ void ConstraintModelNumDiffTpl<Scalar>::calc(
46 const std::shared_ptr<ConstraintDataAbstract>& data,
47 const Eigen::Ref<const VectorXs>& x) {
48 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
54
55 ✗ d->data_0->g.setZero();
56 ✗ d->data_0->h.setZero();
57 ✗ model_->calc(d->data_0, x);
58 ✗ d->g = d->data_0->g;
59 ✗ d->h = d->data_0->h;
60 }
61
62 template <typename Scalar>
63 ✗ 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 ✗ 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 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
77
78 ✗ const VectorXs& g0 = d->g;
79 ✗ const VectorXs& h0 = d->h;
80 ✗ const std::size_t ndx = model_->get_state()->get_ndx();
81 ✗ const std::size_t nu = model_->get_nu();
82 ✗ const std::size_t ng = model_->get_ng();
83 ✗ const std::size_t nh = model_->get_nh();
84 ✗ d->Gx.resize(ng, ndx);
85 ✗ d->Gu.resize(ng, nu);
86 ✗ d->Hx.resize(nh, ndx);
87 ✗ d->Hu.resize(nh, nu);
88 ✗ d->du.setZero();
89
90 ✗ assertStableStateFD(x);
91
92 // Computing the d constraint(x,u) / dx
93 ✗ model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
94 ✗ d->x_norm = d->dx.norm();
95 ✗ d->dx.setZero();
96 ✗ d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
97 ✗ for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
98 ✗ d->dx(ix) = d->xh_jac;
99 ✗ model_->get_state()->integrate(x, d->dx, d->xp);
100 // call the update function
101 ✗ for (size_t i = 0; i < reevals_.size(); ++i) {
102 ✗ reevals_[i](d->xp, u);
103 }
104 ✗ model_->calc(d->data_x[ix], d->xp, u);
105 ✗ d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
106 ✗ d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
107 ✗ d->dx(ix) = Scalar(0.);
108 }
109
110 // Computing the d constraint(x,u) / du
111 ✗ d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
112 ✗ for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
113 ✗ d->du(iu) = d->uh_jac;
114 ✗ d->up = u + d->du;
115 // call the update function
116 ✗ for (std::size_t i = 0; i < reevals_.size(); ++i) {
117 ✗ reevals_[i](x, d->up);
118 }
119 ✗ model_->calc(d->data_u[iu], x, d->up);
120 ✗ d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
121 ✗ d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
122 ✗ d->du(iu) = Scalar(0.);
123 }
124 }
125
126 template <typename Scalar>
127 ✗ void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
128 const std::shared_ptr<ConstraintDataAbstract>& data,
129 const Eigen::Ref<const VectorXs>& x) {
130 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
136
137 ✗ const VectorXs& g0 = d->g;
138 ✗ const VectorXs& h0 = d->h;
139 ✗ const std::size_t ndx = model_->get_state()->get_ndx();
140 ✗ d->Gx.resize(model_->get_ng(), ndx);
141 ✗ d->Hx.resize(model_->get_nh(), ndx);
142
143 ✗ assertStableStateFD(x);
144
145 // Computing the d constraint(x) / dx
146 ✗ model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
147 ✗ d->x_norm = d->dx.norm();
148 ✗ d->dx.setZero();
149 ✗ d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
150 ✗ for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
151 // x + dx
152 ✗ d->dx(ix) = d->xh_jac;
153 ✗ model_->get_state()->integrate(x, d->dx, d->xp);
154 // call the update function
155 ✗ for (size_t i = 0; i < reevals_.size(); ++i) {
156 ✗ reevals_[i](d->xp, unone_);
157 }
158 ✗ model_->calc(d->data_x[ix], d->xp);
159 ✗ d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
160 ✗ d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
161 ✗ d->dx(ix) = Scalar(0.);
162 }
163 }
164
165 template <typename Scalar>
166 std::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
167 ✗ ConstraintModelNumDiffTpl<Scalar>::createData(
168 DataCollectorAbstract* const data) {
169 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
170 ✗ 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 ✗ ConstraintModelNumDiffTpl<Scalar>::get_model() const {
185 ✗ return model_;
186 }
187
188 template <typename Scalar>
189 ✗ const Scalar ConstraintModelNumDiffTpl<Scalar>::get_disturbance() const {
190 ✗ 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 ✗ void ConstraintModelNumDiffTpl<Scalar>::set_reevals(
204 const std::vector<ReevaluationFunction>& reevals) {
205 ✗ reevals_ = reevals;
206 }
207
208 template <typename Scalar>
209 ✗ void ConstraintModelNumDiffTpl<Scalar>::assertStableStateFD(
210 const Eigen::Ref<const VectorXs>& /*x*/) {
211 // do nothing in the general case
212 }
213
214 } // namespace crocoddyl
215