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		✗	ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
14			std::shared_ptr<StateAbstractTpl<Scalar> > 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_(MathBase::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_(MathBase::VectorXs::Constant(
30		✗	nu, -std::numeric_limits<Scalar>::infinity())),
31		✗	u_ub_(MathBase::VectorXs::Constant(
32		✗	nu, std::numeric_limits<Scalar>::infinity())),
33		✗	has_control_limits_(false) {}
34
35			template <typename Scalar>
36		✗	ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
37			const ActionModelAbstractTpl<Scalar>& other)
38		✗	: nu_(other.nu_),
39		✗	nr_(other.nr_),
40		✗	ng_(other.ng_),
41		✗	nh_(other.nh_),
42		✗	ng_T_(other.ng_T_),
43		✗	nh_T_(other.nh_T_),
44		✗	state_(other.state_),
45		✗	g_lb_(other.g_lb_),
46		✗	g_ub_(other.g_ub_),
47		✗	u_lb_(other.u_lb_),
48		✗	u_ub_(other.u_ub_),
49		✗	has_control_limits_(other.has_control_limits_) {}
50
51			template <typename Scalar>
52		✗	void ActionModelAbstractTpl<Scalar>::calc(
53			const std::shared_ptr<ActionDataAbstract>& data,
54			const Eigen::Ref<const VectorXs>& x) {
55		✗	calc(data, x, unone_);
56		✗	}
57
58			template <typename Scalar>
59		✗	void ActionModelAbstractTpl<Scalar>::calcDiff(
60			const std::shared_ptr<ActionDataAbstract>& data,
61			const Eigen::Ref<const VectorXs>& x) {
62		✗	calcDiff(data, x, unone_);
63		✗	}
64
65			template <typename Scalar>
66		✗	void ActionModelAbstractTpl<Scalar>::quasiStatic(
67			const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
68			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
69			const Scalar tol) {
70		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
71		✗	throw_pretty(
72			"Invalid argument: " << "u has wrong dimension (it should be " +
73			std::to_string(nu_) + ")");
74			}
75		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
76		✗	throw_pretty(
77			"Invalid argument: " << "x has wrong dimension (it should be " +
78			std::to_string(state_->get_nx()) + ")");
79			}
80			// Check the velocity input is zero
81		✗	assert_pretty(x.tail(state_->get_nv()).isZero(),
82			"The velocity input should be zero for quasi-static to work.");
83
84		✗	const std::size_t ndx = state_->get_ndx();
85		✗	VectorXs dx = VectorXs::Zero(ndx);
86		✗	if (nu_ != 0) {
87		✗	VectorXs du = VectorXs::Zero(nu_);
88		✗	for (std::size_t i = 0; i < maxiter; ++i) {
89		✗	calc(data, x, u);
90		✗	calcDiff(data, x, u);
91		✗	state_->diff(x, data->xnext, dx);
92		✗	du.noalias() = -pseudoInverse(data->Fu) * dx;
93		✗	u += du;
94		✗	if (du.norm() <= tol) {
95		✗	break;
96			}
97			}
98		✗	}
99		✗	}
100
101			template <typename Scalar>
102			typename MathBaseTpl<Scalar>::VectorXs
103		✗	ActionModelAbstractTpl<Scalar>::quasiStatic_x(
104			const std::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
105			const std::size_t maxiter, const Scalar tol) {
106		✗	VectorXs u(nu_);
107		✗	u.setZero();
108		✗	quasiStatic(data, u, x, maxiter, tol);
109		✗	return u;
110		✗	}
111
112			template <typename Scalar>
113			std::shared_ptr<ActionDataAbstractTpl<Scalar> >
114		✗	ActionModelAbstractTpl<Scalar>::createData() {
115			return std::allocate_shared<ActionDataAbstract>(
116		✗	Eigen::aligned_allocator<ActionDataAbstract>(), this);
117			}
118
119			template <typename Scalar>
120		✗	bool ActionModelAbstractTpl<Scalar>::checkData(
121			const std::shared_ptr<ActionDataAbstract>&) {
122		✗	return false;
123			}
124
125			template <typename Scalar>
126		✗	void ActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
127		✗	os << boost::core::demangle(typeid(*this).name());
128		✗	}
129
130			template <typename Scalar>
131		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_nu() const {
132		✗	return nu_;
133			}
134
135			template <typename Scalar>
136		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_nr() const {
137		✗	return nr_;
138			}
139
140			template <typename Scalar>
141		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_ng() const {
142		✗	return ng_;
143			}
144
145			template <typename Scalar>
146		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_nh() const {
147		✗	return nh_;
148			}
149
150			template <typename Scalar>
151		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_ng_T() const {
152		✗	return ng_T_;
153			}
154
155			template <typename Scalar>
156		✗	std::size_t ActionModelAbstractTpl<Scalar>::get_nh_T() const {
157		✗	return nh_T_;
158			}
159
160			template <typename Scalar>
161			const std::shared_ptr<StateAbstractTpl<Scalar> >&
162		✗	ActionModelAbstractTpl<Scalar>::get_state() const {
163		✗	return state_;
164			}
165
166			template <typename Scalar>
167			const typename MathBaseTpl<Scalar>::VectorXs&
168		✗	ActionModelAbstractTpl<Scalar>::get_g_lb() const {
169		✗	return g_lb_;
170			}
171
172			template <typename Scalar>
173			const typename MathBaseTpl<Scalar>::VectorXs&
174		✗	ActionModelAbstractTpl<Scalar>::get_g_ub() const {
175		✗	return g_ub_;
176			}
177
178			template <typename Scalar>
179			const typename MathBaseTpl<Scalar>::VectorXs&
180		✗	ActionModelAbstractTpl<Scalar>::get_u_lb() const {
181		✗	return u_lb_;
182			}
183
184			template <typename Scalar>
185			const typename MathBaseTpl<Scalar>::VectorXs&
186		✗	ActionModelAbstractTpl<Scalar>::get_u_ub() const {
187		✗	return u_ub_;
188			}
189
190			template <typename Scalar>
191		✗	bool ActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
192		✗	return has_control_limits_;
193			}
194
195			template <typename Scalar>
196		✗	void ActionModelAbstractTpl<Scalar>::set_g_lb(const VectorXs& g_lb) {
197		✗	const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
198		✗	if (static_cast<std::size_t>(g_lb.size()) != ng) {
199		✗	throw_pretty(
200			"Invalid argument: "
201			<< "inequality lower bound has wrong dimension (it should be " +
202			std::to_string(ng) + ")");
203			}
204		✗	g_lb_ = g_lb;
205		✗	}
206
207			template <typename Scalar>
208		✗	void ActionModelAbstractTpl<Scalar>::set_g_ub(const VectorXs& g_ub) {
209		✗	const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
210		✗	if (static_cast<std::size_t>(g_ub.size()) != ng) {
211		✗	throw_pretty(
212			"Invalid argument: "
213			<< "inequality upper bound has wrong dimension (it should be " +
214			std::to_string(ng_) + ")");
215			}
216		✗	g_ub_ = g_ub;
217		✗	}
218
219			template <typename Scalar>
220		✗	void ActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
221		✗	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
222		✗	throw_pretty("Invalid argument: "
223			<< "control lower bound has wrong dimension (it should be " +
224			std::to_string(nu_) + ")");
225			}
226		✗	u_lb_ = u_lb;
227		✗	update_has_control_limits();
228		✗	}
229
230			template <typename Scalar>
231		✗	void ActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
232		✗	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
233		✗	throw_pretty("Invalid argument: "
234			<< "control upper bound has wrong dimension (it should be " +
235			std::to_string(nu_) + ")");
236			}
237		✗	u_ub_ = u_ub;
238		✗	update_has_control_limits();
239		✗	}
240
241			template <typename Scalar>
242		✗	void ActionModelAbstractTpl<Scalar>::update_has_control_limits() {
243		✗	has_control_limits_ =
244		✗	isfinite(u_lb_.template cast<ScalarType>().array()).any() &&
245		✗	isfinite(u_ub_.template cast<ScalarType>().array()).any();
246		✗	}
247
248			template <typename Scalar>
249		✗	std::ostream& operator<<(std::ostream& os,
250			const ActionModelAbstractTpl<Scalar>& model) {
251		✗	model.print(os);
252		✗	return os;
253			}
254
255			} // namespace crocoddyl
256