GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	DifferentialActionModelAbstractTpl<Scalar>::DifferentialActionModelAbstractTpl(
14			std::shared_ptr<StateAbstract> state, const std::size_t nu,
15			const std::size_t nr, const std::size_t ng, const std::size_t nh,
16			const std::size_t ng_T, const std::size_t nh_T)
17		✗	: nu_(nu),
18		✗	nr_(nr),
19		✗	ng_(ng),
20		✗	nh_(nh),
21		✗	ng_T_(ng_T),
22		✗	nh_T_(nh_T),
23		✗	state_(state),
24		✗	unone_(VectorXs::Zero(nu)),
25		✗	g_lb_(VectorXs::Constant(ng > ng_T ? ng : ng_T,
26		✗	-std::numeric_limits<Scalar>::infinity())),
27		✗	g_ub_(VectorXs::Constant(ng > ng_T ? ng : ng_T,
28		✗	std::numeric_limits<Scalar>::infinity())),
29		✗	u_lb_(VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
30		✗	u_ub_(VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
31		✗	has_control_limits_(false) {}
32
33			template <typename Scalar>
34		✗	void DifferentialActionModelAbstractTpl<Scalar>::calc(
35			const std::shared_ptr<DifferentialActionDataAbstract>& data,
36			const Eigen::Ref<const VectorXs>& x) {
37		✗	calc(data, x, unone_);
38		✗	}
39
40			template <typename Scalar>
41		✗	void DifferentialActionModelAbstractTpl<Scalar>::calcDiff(
42			const std::shared_ptr<DifferentialActionDataAbstract>& data,
43			const Eigen::Ref<const VectorXs>& x) {
44		✗	calcDiff(data, x, unone_);
45		✗	}
46
47			template <typename Scalar>
48		✗	void DifferentialActionModelAbstractTpl<Scalar>::quasiStatic(
49			const std::shared_ptr<DifferentialActionDataAbstract>& data,
50			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
51			const std::size_t maxiter, const Scalar tol) {
52		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
53		✗	throw_pretty(
54			"Invalid argument: " << "u has wrong dimension (it should be " +
55			std::to_string(nu_) + ")");
56			}
57		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
58		✗	throw_pretty(
59			"Invalid argument: " << "x has wrong dimension (it should be " +
60			std::to_string(state_->get_nx()) + ")");
61			}
62			// Check the velocity input is zero
63		✗	assert_pretty(x.tail(state_->get_nv()).isZero(),
64			"The velocity input should be zero for quasi-static to work.");
65
66		✗	if (nu_ != 0) {
67		✗	VectorXs du = VectorXs::Zero(nu_);
68		✗	for (std::size_t i = 0; i < maxiter; ++i) {
69		✗	calc(data, x, u);
70		✗	calcDiff(data, x, u);
71		✗	du.noalias() = -pseudoInverse(data->Fu) * data->xout;
72		✗	u += du;
73		✗	if (du.norm() <= tol) {
74		✗	break;
75			}
76			}
77		✗	}
78		✗	}
79
80			template <typename Scalar>
81			typename MathBaseTpl<Scalar>::VectorXs
82		✗	DifferentialActionModelAbstractTpl<Scalar>::quasiStatic_x(
83			const std::shared_ptr<DifferentialActionDataAbstract>& data,
84			const VectorXs& x, const std::size_t maxiter, const Scalar tol) {
85		✗	VectorXs u(nu_);
86		✗	u.setZero();
87		✗	quasiStatic(data, u, x, maxiter, tol);
88		✗	return u;
89		✗	}
90
91			template <typename Scalar>
92			std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
93		✗	DifferentialActionModelAbstractTpl<Scalar>::createData() {
94			return std::allocate_shared<DifferentialActionDataAbstract>(
95		✗	Eigen::aligned_allocator<DifferentialActionDataAbstract>(), this);
96			}
97
98			template <typename Scalar>
99		✗	bool DifferentialActionModelAbstractTpl<Scalar>::checkData(
100			const std::shared_ptr<DifferentialActionDataAbstract>&) {
101		✗	return false;
102			}
103
104			template <typename Scalar>
105		✗	void DifferentialActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
106		✗	os << boost::core::demangle(typeid(*this).name());
107		✗	}
108
109			template <typename Scalar>
110		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nu() const {
111		✗	return nu_;
112			}
113
114			template <typename Scalar>
115		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nr() const {
116		✗	return nr_;
117			}
118
119			template <typename Scalar>
120		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_ng() const {
121		✗	return ng_;
122			}
123
124			template <typename Scalar>
125		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nh() const {
126		✗	return nh_;
127			}
128
129			template <typename Scalar>
130		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_ng_T() const {
131		✗	return ng_T_;
132			}
133
134			template <typename Scalar>
135		✗	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nh_T() const {
136		✗	return nh_T_;
137			}
138
139			template <typename Scalar>
140			const std::shared_ptr<StateAbstractTpl<Scalar> >&
141		✗	DifferentialActionModelAbstractTpl<Scalar>::get_state() const {
142		✗	return state_;
143			}
144
145			template <typename Scalar>
146			const typename MathBaseTpl<Scalar>::VectorXs&
147		✗	DifferentialActionModelAbstractTpl<Scalar>::get_g_lb() const {
148		✗	return g_lb_;
149			}
150
151			template <typename Scalar>
152			const typename MathBaseTpl<Scalar>::VectorXs&
153		✗	DifferentialActionModelAbstractTpl<Scalar>::get_g_ub() const {
154		✗	return g_ub_;
155			}
156
157			template <typename Scalar>
158			const typename MathBaseTpl<Scalar>::VectorXs&
159		✗	DifferentialActionModelAbstractTpl<Scalar>::get_u_lb() const {
160		✗	return u_lb_;
161			}
162
163			template <typename Scalar>
164			const typename MathBaseTpl<Scalar>::VectorXs&
165		✗	DifferentialActionModelAbstractTpl<Scalar>::get_u_ub() const {
166		✗	return u_ub_;
167			}
168
169			template <typename Scalar>
170		✗	bool DifferentialActionModelAbstractTpl<Scalar>::get_has_control_limits()
171			const {
172		✗	return has_control_limits_;
173			}
174
175			template <typename Scalar>
176		✗	void DifferentialActionModelAbstractTpl<Scalar>::set_g_lb(
177			const VectorXs& g_lb) {
178		✗	const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
179		✗	if (static_cast<std::size_t>(g_lb.size()) != ng) {
180		✗	throw_pretty(
181			"Invalid argument: "
182			<< "inequality lower bound has wrong dimension (it should be " +
183			std::to_string(ng) + ")");
184			}
185		✗	g_lb_ = g_lb;
186		✗	}
187
188			template <typename Scalar>
189		✗	void DifferentialActionModelAbstractTpl<Scalar>::set_g_ub(
190			const VectorXs& g_ub) {
191		✗	const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
192		✗	if (static_cast<std::size_t>(g_ub.size()) != ng) {
193		✗	throw_pretty(
194			"Invalid argument: "
195			<< "inequality upper bound has wrong dimension (it should be " +
196			std::to_string(ng_) + ")");
197			}
198		✗	g_ub_ = g_ub;
199		✗	}
200
201			template <typename Scalar>
202		✗	void DifferentialActionModelAbstractTpl<Scalar>::set_u_lb(
203			const VectorXs& u_lb) {
204		✗	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
205		✗	throw_pretty("Invalid argument: "
206			<< "lower bound has wrong dimension (it should be " +
207			std::to_string(nu_) + ")");
208			}
209		✗	u_lb_ = u_lb;
210		✗	update_has_control_limits();
211		✗	}
212
213			template <typename Scalar>
214		✗	void DifferentialActionModelAbstractTpl<Scalar>::set_u_ub(
215			const VectorXs& u_ub) {
216		✗	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
217		✗	throw_pretty("Invalid argument: "
218			<< "upper bound has wrong dimension (it should be " +
219			std::to_string(nu_) + ")");
220			}
221		✗	u_ub_ = u_ub;
222		✗	update_has_control_limits();
223		✗	}
224
225			template <typename Scalar>
226		✗	void DifferentialActionModelAbstractTpl<Scalar>::update_has_control_limits() {
227		✗	has_control_limits_ =
228		✗	isfinite(u_lb_.template cast<ScalarType>().array()).any() &&
229		✗	isfinite(u_ub_.template cast<ScalarType>().array()).any();
230		✗	}
231
232			template <typename Scalar>
233		✗	std::ostream& operator<<(
234			std::ostream& os, const DifferentialActionModelAbstractTpl<Scalar>& model) {
235		✗	model.print(os);
236		✗	return os;
237			}
238
239			} // namespace crocoddyl
240