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			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11			template <typename Scalar>
12		✗	StateAbstractTpl<Scalar>::StateAbstractTpl(const std::size_t nx,
13			const std::size_t ndx)
14		✗	: nx_(nx),
15		✗	ndx_(ndx),
16		✗	lb_(VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
17		✗	ub_(VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
18		✗	has_limits_(false) {
19		✗	nv_ = ndx / 2;
20		✗	nq_ = nx_ - nv_;
21		✗	}
22
23			template <typename Scalar>
24		✗	StateAbstractTpl<Scalar>::StateAbstractTpl()
25		✗	: nx_(0),
26		✗	ndx_(0),
27		✗	lb_(MathBase::VectorXs::Constant(
28		✗	nx_, -std::numeric_limits<Scalar>::infinity())),
29		✗	ub_(MathBase::VectorXs::Constant(
30		✗	nx_, std::numeric_limits<Scalar>::infinity())),
31		✗	has_limits_(false) {}
32
33			template <typename Scalar>
34		✗	StateAbstractTpl<Scalar>::~StateAbstractTpl() {}
35
36			template <typename Scalar>
37		✗	typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::diff_dx(
38			const Eigen::Ref<const VectorXs>& x0,
39			const Eigen::Ref<const VectorXs>& x1) {
40		✗	VectorXs dxout = VectorXs::Zero(ndx_);
41		✗	diff(x0, x1, dxout);
42		✗	return dxout;
43		✗	}
44
45			template <typename Scalar>
46		✗	typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::integrate_x(
47			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx) {
48		✗	VectorXs xout = VectorXs::Zero(nx_);
49		✗	integrate(x, dx, xout);
50		✗	return xout;
51		✗	}
52
53			template <typename Scalar>
54			std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
55		✗	StateAbstractTpl<Scalar>::Jdiff_Js(const Eigen::Ref<const VectorXs>& x0,
56			const Eigen::Ref<const VectorXs>& x1,
57			Jcomponent firstsecond) {
58		✗	MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
59		✗	Jfirst.setZero();
60		✗	Jsecond.setZero();
61		✗	std::vector<MatrixXs> Jacs;
62		✗	Jdiff(x0, x1, Jfirst, Jsecond, firstsecond);
63		✗	switch (firstsecond) {
64		✗	case both:
65		✗	Jacs.push_back(Jfirst);
66		✗	Jacs.push_back(Jsecond);
67		✗	break;
68		✗	case first:
69		✗	Jacs.push_back(Jfirst);
70		✗	break;
71		✗	case second:
72		✗	Jacs.push_back(Jsecond);
73		✗	break;
74		✗	default:
75		✗	Jacs.push_back(Jfirst);
76		✗	Jacs.push_back(Jsecond);
77		✗	break;
78			}
79		✗	return Jacs;
80		✗	}
81
82			template <typename Scalar>
83			std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
84		✗	StateAbstractTpl<Scalar>::Jintegrate_Js(const Eigen::Ref<const VectorXs>& x,
85			const Eigen::Ref<const VectorXs>& dx,
86			const Jcomponent firstsecond) {
87		✗	MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
88		✗	Jfirst.setZero();
89		✗	Jsecond.setZero();
90		✗	std::vector<MatrixXs> Jacs;
91		✗	Jintegrate(x, dx, Jfirst, Jsecond, firstsecond, setto);
92		✗	switch (firstsecond) {
93		✗	case both:
94		✗	Jacs.push_back(Jfirst);
95		✗	Jacs.push_back(Jsecond);
96		✗	break;
97		✗	case first:
98		✗	Jacs.push_back(Jfirst);
99		✗	break;
100		✗	case second:
101		✗	Jacs.push_back(Jsecond);
102		✗	break;
103		✗	default:
104		✗	Jacs.push_back(Jfirst);
105		✗	Jacs.push_back(Jsecond);
106		✗	break;
107			}
108		✗	return Jacs;
109		✗	}
110
111			template <typename Scalar>
112		✗	void StateAbstractTpl<Scalar>::print(std::ostream& os) const {
113		✗	os << boost::core::demangle(typeid(*this).name());
114		✗	}
115
116			template <typename Scalar>
117		✗	std::size_t StateAbstractTpl<Scalar>::get_nx() const {
118		✗	return nx_;
119			}
120
121			template <typename Scalar>
122		✗	std::size_t StateAbstractTpl<Scalar>::get_ndx() const {
123		✗	return ndx_;
124			}
125
126			template <typename Scalar>
127		✗	std::size_t StateAbstractTpl<Scalar>::get_nq() const {
128		✗	return nq_;
129			}
130
131			template <typename Scalar>
132		✗	std::size_t StateAbstractTpl<Scalar>::get_nv() const {
133		✗	return nv_;
134			}
135
136			template <typename Scalar>
137		✗	const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_lb()
138			const {
139		✗	return lb_;
140			}
141
142			template <typename Scalar>
143		✗	const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_ub()
144			const {
145		✗	return ub_;
146			}
147
148			template <typename Scalar>
149		✗	bool StateAbstractTpl<Scalar>::get_has_limits() const {
150		✗	return has_limits_;
151			}
152
153			template <typename Scalar>
154		✗	void StateAbstractTpl<Scalar>::set_lb(const VectorXs& lb) {
155		✗	if (static_cast<std::size_t>(lb.size()) != nx_) {
156		✗	throw_pretty("Invalid argument: "
157			<< "lower bound has wrong dimension (it should be " +
158			std::to_string(nx_) + ")");
159			}
160		✗	lb_ = lb;
161		✗	update_has_limits();
162		✗	}
163
164			template <typename Scalar>
165		✗	void StateAbstractTpl<Scalar>::set_ub(const VectorXs& ub) {
166		✗	if (static_cast<std::size_t>(ub.size()) != nx_) {
167		✗	throw_pretty("Invalid argument: "
168			<< "upper bound has wrong dimension (it should be " +
169			std::to_string(nx_) + ")");
170			}
171		✗	ub_ = ub;
172		✗	update_has_limits();
173		✗	}
174
175			template <typename Scalar>
176		✗	void StateAbstractTpl<Scalar>::update_has_limits() {
177		✗	has_limits_ = isfinite(lb_.array()).any() || isfinite(ub_.array()).any();
178		✗	}
179
180			template <typename Scalar>
181		✗	std::ostream& operator<<(std::ostream& os,
182			const StateAbstractTpl<Scalar>& model) {
183		✗	model.print(os);
184		✗	return os;
185			}
186
187			} // namespace crocoddyl
188