GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, University of Edinburgh, University of Oxford,
5			// 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		✗	CoPSupportTpl<Scalar>::CoPSupportTpl(const Matrix3s& R, const Vector2s& box)
14		✗	: R_(R), box_(box) {
15		✗	A_.setZero();
16		✗	ub_.setZero();
17		✗	lb_.setZero();
18
19			// Update the inequality matrix and bounds
20		✗	update();
21		✗	}
22
23			template <typename Scalar>
24		✗	CoPSupportTpl<Scalar>::CoPSupportTpl(const WrenchConeTpl<Scalar>& other)
25		✗	: A_(other.get_A()),
26		✗	ub_(other.get_ub()),
27		✗	lb_(other.get_lb()),
28		✗	R_(other.get_R()),
29		✗	box_(other.get_box()) {}
30
31			template <typename Scalar>
32		✗	CoPSupportTpl<Scalar>::CoPSupportTpl(const CoPSupportTpl<Scalar>& other)
33		✗	: A_(other.get_A()),
34		✗	ub_(other.get_ub()),
35		✗	lb_(other.get_lb()),
36		✗	R_(other.get_R()),
37		✗	box_(other.get_box()) {}
38
39			template <typename Scalar>
40		✗	CoPSupportTpl<Scalar>::CoPSupportTpl()
41		✗	: R_(Matrix3s::Identity()),
42		✗	box_(std::numeric_limits<Scalar>::infinity(),
43		✗	std::numeric_limits<Scalar>::infinity()) {
44		✗	A_.setZero();
45		✗	ub_.setZero();
46		✗	lb_.setZero();
47
48			// Update the inequality matrix and bounds
49		✗	update();
50		✗	}
51
52			template <typename Scalar>
53		✗	CoPSupportTpl<Scalar>::~CoPSupportTpl() {}
54
55			template <typename Scalar>
56		✗	void CoPSupportTpl<Scalar>::update() {
57			// Initialize the matrix and bounds
58		✗	A_.setZero();
59		✗	ub_.setZero();
60		✗	lb_.setOnes();
61		✗	lb_ *= -std::numeric_limits<Scalar>::infinity();
62
63			// CoP information
64			// This matrix is defined as
65			// [0 0 -W 1 0 0;
66			// 0 0 -W -1 0 0;
67			// 0 0 -L 0 1 0;
68			// 0 0 -L 0 -1 0]
69		✗	const Scalar L = box_(0) / Scalar(2.);
70		✗	const Scalar W = box_(1) / Scalar(2.);
71		✗	A_.row(0) << -W * R_.col(2).transpose(), R_.col(0).transpose();
72		✗	A_.row(1) << -W * R_.col(2).transpose(), -R_.col(0).transpose();
73		✗	A_.row(2) << -L * R_.col(2).transpose(), R_.col(1).transpose();
74		✗	A_.row(3) << -L * R_.col(2).transpose(), -R_.col(1).transpose();
75		✗	}
76
77			template <typename Scalar>
78			template <typename NewScalar>
79		✗	CoPSupportTpl<NewScalar> CoPSupportTpl<Scalar>::cast() const {
80			typedef CoPSupportTpl<NewScalar> ReturnType;
81		✗	ReturnType ret(R_.template cast<NewScalar>(),
82		✗	box_.template cast<NewScalar>());
83		✗	return ret;
84			}
85
86			template <typename Scalar>
87		✗	const typename MathBaseTpl<Scalar>::Matrix46s& CoPSupportTpl<Scalar>::get_A()
88			const {
89		✗	return A_;
90			}
91
92			template <typename Scalar>
93		✗	const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_ub()
94			const {
95		✗	return ub_;
96			}
97
98			template <typename Scalar>
99		✗	const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_lb()
100			const {
101		✗	return lb_;
102			}
103
104			template <typename Scalar>
105		✗	const typename MathBaseTpl<Scalar>::Vector2s& CoPSupportTpl<Scalar>::get_box()
106			const {
107		✗	return box_;
108			}
109
110			template <typename Scalar>
111		✗	const typename MathBaseTpl<Scalar>::Matrix3s& CoPSupportTpl<Scalar>::get_R()
112			const {
113		✗	return R_;
114			}
115
116			template <typename Scalar>
117		✗	void CoPSupportTpl<Scalar>::set_R(const Matrix3s& R) {
118		✗	R_ = R;
119		✗	}
120
121			template <typename Scalar>
122		✗	void CoPSupportTpl<Scalar>::set_box(const Vector2s& box) {
123		✗	box_ = box;
124		✗	if (box_(0) < Scalar(0.)) {
125		✗	box_(0) = std::numeric_limits<Scalar>::infinity();
126		✗	std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
127		✗	<< std::endl;
128			}
129		✗	if (box_(1) < Scalar(0.)) {
130		✗	box_(1) = std::numeric_limits<Scalar>::infinity();
131		✗	std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
132		✗	<< std::endl;
133			}
134		✗	}
135
136			template <typename Scalar>
137		✗	CoPSupportTpl<Scalar>& CoPSupportTpl<Scalar>::operator=(
138			const CoPSupportTpl<Scalar>& other) {
139		✗	if (this != &other) {
140		✗	A_ = other.get_A();
141		✗	ub_ = other.get_ub();
142		✗	lb_ = other.get_lb();
143		✗	R_ = other.get_R();
144		✗	box_ = other.get_box();
145			}
146		✗	return *this;
147			}
148
149			template <typename Scalar>
150		✗	std::ostream& operator<<(std::ostream& os, const CoPSupportTpl<Scalar>& X) {
151		✗	os << " R: " << X.get_R() << std::endl;
152		✗	os << " box: " << X.get_box().transpose() << std::endl;
153		✗	return os;
154			}
155
156			} // namespace crocoddyl
157