GCC Code Coverage Report

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