GCC Code Coverage Report

Directory: ./
File: include/multicontact-api/geometry/linear-cone.hpp
Date: 2025-04-05 01:06:26
Exec Total Coverage
Lines: 44 88 50.0%
Branches: 33 126 26.2%
Line Branch Exec Source
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3
4 #ifndef __multicontact_api_geometry_linear_cone_hpp__
5 #define __multicontact_api_geometry_linear_cone_hpp__
6
7 #include <Eigen/Dense>
8 #include <Eigen/Geometry>
9 #include <pinocchio/spatial/se3.hpp>
10
11 #include "multicontact-api/geometry/fwd.hpp"
12 #include "multicontact-api/serialization/archive.hpp"
13 #include "multicontact-api/serialization/eigen-matrix.hpp"
14
15 #define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(TYPE, ROWS) \
16 EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS, \
17 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
18
19 namespace multicontact_api {
20 namespace geometry {
21
22 template <typename _Scalar, int _dim, int _Options>
23 struct LinearCone
24 : public serialization::Serializable<LinearCone<_Scalar, _dim, _Options> > {
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
27 enum { dim = _dim, Options = _Options };
28 typedef Eigen::Matrix<Scalar, dim, -1, Options> MatrixDx;
29 typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
30 typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
31 typedef Eigen::DenseIndex Index;
32
33 /// \brief Default constructor
34 ✗ LinearCone() : m_rays() {}
35
36 /// \brief Constructor from a set of rays
37 template <typename EigenDerived>
38 1 explicit LinearCone(const Eigen::MatrixBase<EigenDerived>& rays)
39 // : m_rays(_dim,rays.cols())
40 1 : m_rays(rays) {
41 // EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(EigenDerived,MatrixDx)
42 // for(int k=0; k<rays.cols(); ++k)
43 // m_rays.col(k) = rays.col(k).normalized();
44 1 }
45
46 /// \brief Contrustor from a given size.
47
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 explicit LinearCone(const Index size) : m_rays(_dim, size) {}
48
49 /// \brief Copy constructor
50 template <typename S2, int O2>
51 LinearCone(const LinearCone<S2, dim, O2>& other) : m_rays(other.m_rays) {}
52
53 void addRay(const VectorD& ray) {
54 m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + 1);
55 m_rays.template rightCols<1>() = ray.normalized();
56 }
57
58 template <typename EigenDerived>
59 ✗ void stack(const Eigen::MatrixBase<EigenDerived>& rays) {
60 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(EigenDerived, dim);
61 ✗ m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + rays.cols());
62 ✗ m_rays.rightCols(rays.cols()) = rays;
63 }
64
65 template <typename S2, int O2>
66 ✗ void stack(const LinearCone<S2, dim, O2>& other) {
67 ✗ stack(other.rays());
68 }
69
70 /// \returns the rays of the linear cone.
71 60 const MatrixDx& rays() const { return m_rays; }
72 34 MatrixDx& rays() { return m_rays; }
73
74 /// \returns the number of rays, i.e. the number of cols of m_rays
75 74 Index size() const { return m_rays.cols(); }
76
77 template <typename S2, int O2>
78 ✗ bool operator==(const LinearCone<S2, dim, O2>& other) const {
79 ✗ return m_rays == other.m_rays;
80 }
81
82 template <typename S2, int O2>
83 ✗ bool operator!=(const LinearCone<S2, dim, O2>& other) const {
84 ✗ return !(*this == other);
85 }
86
87 template <typename S2, int O2>
88 4 bool isApprox(
89 const LinearCone<S2, dim, O2>& other,
90 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
91 4 return m_rays.isApprox(other.m_rays, prec);
92 }
93
94 ✗ void disp(std::ostream& os) const { os << "Rays:\n" << m_rays << std::endl; }
95
96 ✗ friend std::ostream& operator<<(std::ostream& os, const LinearCone& C) {
97 ✗ C.disp(os);
98 ✗ return os;
99 }
100
101 protected:
102 /// \brief Rays of the linear cone
103 MatrixDx m_rays;
104
105 private:
106 // Serialization of the class
107 friend class boost::serialization::access;
108
109 template <class Archive>
110 6 void save(Archive& ar, const unsigned int /*version*/) const {
111
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar& boost::serialization::make_nvp("rays", m_rays);
112 6 }
113
114 template <class Archive>
115 6 void load(Archive& ar, const unsigned int /*version*/) {
116
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("rays", m_rays);
117 6 }
118
119 12 BOOST_SERIALIZATION_SPLIT_MEMBER()
120 };
121
122 template <typename _Scalar, int _Options>
123 struct ForceConeTpl : public LinearCone<_Scalar, 3, _Options> {
124 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
125
126 typedef LinearCone<_Scalar, 3, _Options> Base;
127 typedef WrenchConeTpl<_Scalar, _Options> WrenchCone;
128 typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
129 using typename Base::Scalar;
130 enum { dim = Base::dim };
131 using Base::rays;
132 using Base::size;
133 using typename Base::Index;
134 using typename Base::MatrixD;
135 using typename Base::MatrixDx;
136 using typename Base::VectorD;
137 using Base::operator==;
138 using Base::operator!=;
139 // using Base::isApprox; // Leads to a bug with clang
140
141 typedef MatrixDx Matrix3x;
142 typedef VectorD Vector3;
143 typedef Eigen::AngleAxis<Scalar> AngleAxis;
144
145 /// \brief Default constructor
146 ✗ ForceConeTpl() : Base() {}
147
148 template <typename EigenDerived>
149 1 explicit ForceConeTpl(const Eigen::MatrixBase<EigenDerived>& rays)
150 1 : Base(rays) {}
151
152 3 explicit ForceConeTpl(const Index size) : Base(size) {}
153
154 /// \returns a linear cone built from a friction coefficient and the number of
155 /// rays along the Z axis.
156 ✗ static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction,
157 const int num_rays) {
158 ✗ return RegularCone(mu, direction, num_rays, 0.);
159 }
160
161 ✗ static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction,
162 const int num_rays,
163 const Scalar theta_offset) {
164 ✗ assert(mu >= 0. && "mu must be positive");
165 ✗ assert(num_rays >= 1 && "The number of rays must be at least one");
166
167 ✗ const VectorD normalized_direction(direction.normalized());
168 ✗ ForceConeTpl cone(num_rays);
169
170 ✗ const Scalar angle = (2. * M_PI) / num_rays;
171
172 ✗ const MatrixD Po(MatrixD::Identity() -
173 ✗ normalized_direction * normalized_direction.transpose());
174
175 ✗ const MatrixD rot_offset(
176 ✗ AngleAxis(theta_offset, normalized_direction).toRotationMatrix());
177 ✗ const VectorD init_direction(rot_offset *
178 (Po * VectorD::Ones()).normalized());
179 ✗ const MatrixD rot(
180 ✗ AngleAxis(angle, normalized_direction).toRotationMatrix());
181
182 ✗ VectorD ray((direction + mu * init_direction).normalized());
183
184 ✗ for (int k = 0; k < num_rays; ++k) {
185 ✗ cone.rays().col(k) = ray;
186 ✗ if (k != num_rays - 1) ray = rot * ray;
187 }
188
189 ✗ return cone;
190 }
191
192 2 WrenchCone SE3ActOn(const SE3& M) const {
193 2 WrenchCone res(size());
194 typedef typename WrenchCone::MatrixDx::ColXpr Col6Xpr;
195 typedef typename MatrixDx::ConstColXpr ConstCol3Xpr;
196
197
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename SE3::Matrix3& R = M.rotation();
198
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename SE3::Vector3& t = M.translation();
199
200
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 2 times.
 22 for (Index k = 0; k < size(); ++k) {
201
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 ConstCol3Xpr in_col = rays().col(k);
202
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 Col6Xpr out_col = res.rays().col(k);
203
204
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 20 out_col.template head<3>() = R * in_col;
205
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 out_col.template tail<3>() = t.cross(out_col.template head<3>());
206 }
207
208 2 return res;
209 }
210
211 template <typename S2, int O2>
212 4 bool isApprox(
213 const ForceConeTpl<S2, O2>& other,
214 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
215 4 return Base::isApprox(other, prec);
216 }
217
218 operator WrenchCone() const {
219 WrenchCone res(size());
220 res.rays().template topRows<3>() = rays();
221 res.rays().template bottomRows<3>().setZero();
222 return res;
223 }
224 };
225
226 template <typename _Scalar, int _Options>
227 struct WrenchConeTpl : public LinearCone<_Scalar, 6, _Options> {
228 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
229
230 typedef LinearCone<_Scalar, 6, _Options> Base;
231 typedef ForceConeTpl<_Scalar, _Options> ForceCone;
232 typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
233 using typename Base::Scalar;
234 enum { dim = Base::dim };
235 using Base::rays;
236 using Base::size;
237 using typename Base::Index;
238 using typename Base::MatrixDx;
239 using typename Base::VectorD;
240 using Base::operator==;
241 using Base::operator!=;
242 // using Base::isApprox; // Leads to a bug with clang
243
244 typedef MatrixDx Matrix6x;
245 typedef VectorD Vector6;
246
247 typedef typename ForceCone::Matrix3x Matrix3x;
248
249 typedef typename Matrix6x::template NRowsBlockXpr<3>::Type LinearBlock;
250 typedef
251 typename Matrix6x::template ConstNRowsBlockXpr<3>::Type ConstLinearBlock;
252
253 typedef LinearBlock AngularBlock;
254 typedef ConstLinearBlock ConstAngularBlock;
255
256 /// \brief Default constructor
257 ✗ WrenchConeTpl() : Base() {}
258
259 /// \brief Constructor from a set of rays.
260 template <typename EigenDerived>
261 ✗ explicit WrenchConeTpl(const Eigen::MatrixBase<EigenDerived>& rays)
262 ✗ : Base(rays) {}
263
264 /// \brief Constructs a WrenchCone of a given size.
265 3 explicit WrenchConeTpl(const Index size) : Base(size) {}
266
267 /// \brief Constructs a WrenchCone of a given size.
268 template <typename S2, int O2>
269 ✗ explicit WrenchConeTpl(const ForceConeTpl<S2, O2>& force_cone)
270 ✗ : Base(force_cone.size()) {
271 ✗ rays().template topRows<3>() = force_cone.rays();
272 ✗ rays().template bottomRows<3>().setZero();
273 }
274
275 /// \brief Copy constructor
276 template <typename S2, int O2>
277 WrenchConeTpl(const WrenchConeTpl<S2, O2>& other) : Base(other) {}
278
279 1 WrenchConeTpl SE3ActOn(const SE3& M) const {
280 1 WrenchConeTpl res(size());
281 typedef typename MatrixDx::ColXpr Col6Xpr;
282 typedef typename MatrixDx::ConstColXpr ConstCol6Xpr;
283
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const typename SE3::Matrix3& R = M.rotation();
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const typename SE3::Vector3& t = M.translation();
286
287
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (Index k = 0; k < size(); ++k) {
288
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 ConstCol6Xpr in_col = rays().col(k);
289
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 Col6Xpr out_col = res.rays().col(k);
290
291
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 10 out_col.template head<3>() = R * in_col.template head<3>();
292
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 10 out_col.template tail<3>() =
293
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 t.cross(out_col.template head<3>()) + R * in_col.template tail<3>();
294 }
295
296 1 return res;
297 }
298
299 template <typename S2, int O2>
300 bool isApprox(
301 const WrenchConeTpl<S2, O2>& other,
302 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
303 return Base::isApprox(other, prec);
304 }
305
306 ✗ ConstLinearBlock linear() const { return rays().template topRows<3>(); }
307 2 LinearBlock linear() { return rays().template topRows<3>(); }
308
309 ✗ ConstAngularBlock angular() const { return rays().template bottomRows<3>(); }
310 AngularBlock angular() { return rays().template bottomRows<3>(); }
311
312 ForceCone toForceCone() const { return ForceCone(linear()); }
313 };
314 } // namespace geometry
315 } // namespace multicontact_api
316
317 #endif // ifndef __multicontact_api_geometry_linear_cone_hpp__
318