GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constraints/coulomb-friction-cone.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 74 92 80.4%
Branches: 64 162 39.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
6 #define __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
7
8 #include "pinocchio/algorithm/constraints/fwd.hpp"
9 #include "pinocchio/math/fwd.hpp"
10 #include "pinocchio/math/comparison-operators.hpp"
11
12 namespace pinocchio
13 {
14
15 template<typename Scalar>
16 struct DualCoulombFrictionConeTpl;
17
18 ///  \brief 3d Coulomb friction cone.
19 template<typename _Scalar>
20 struct CoulombFrictionConeTpl
21 {
22 typedef _Scalar Scalar;
23 typedef DualCoulombFrictionConeTpl<Scalar> DualCone;
24 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
25
26 /// \brief Default constructor
27 ///
28 /// \param[in] mu Friction coefficient
29 15 explicit CoulombFrictionConeTpl(const Scalar mu)
30 15 : mu(mu)
31 {
32
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 assert(mu >= 0 && "mu must be positive");
33 15 }
34
35 /// \brief Copy constructor.
36 CoulombFrictionConeTpl(const CoulombFrictionConeTpl & other) = default;
37
38 /// \brief Copy operator
39 CoulombFrictionConeTpl & operator=(const CoulombFrictionConeTpl & other) = default;
40
41 /// \brief Comparison operator
42 ✗ bool operator==(const CoulombFrictionConeTpl & other) const
43 {
44 ✗ return mu == other.mu;
45 }
46
47 /// \brief Difference operator
48 ✗ bool operator!=(const CoulombFrictionConeTpl & other) const
49 {
50 ✗ return !(*this == other);
51 }
52
53 /// \brief Check whether a vector x lies within the cone.
54 ///
55 /// \param[in] f vector to check (assimilated to a force vector).
56 ///
57 template<typename Vector3Like>
58 800002 bool isInside(const Eigen::MatrixBase<Vector3Like> & f, const Scalar prec = Scalar(0)) const
59 {
60
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 400001 times.
 800002 assert(mu >= 0 && "mu must be positive");
61
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 400001 times.
 800002 assert(prec >= 0 && "prec should be positive");
62 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
63
2/4
✓ Branch 2 taken 400001 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400001 times.
✗ Branch 6 not taken.
 800002 return f.template head<2>().norm() <= mu * f[2] + prec;
64 }
65
66 /// \brief Project a vector x onto the cone.
67 ///
68 /// \param[in] x a 3d vector to project.
69 ///
70 template<typename Vector3Like>
71 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
72 400010 project(const Eigen::MatrixBase<Vector3Like> & x) const
73 {
74
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 200009 times.
 400010 assert(mu >= 0 && "mu must be positive");
75 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
76 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
77
1/2
✓ Branch 1 taken 200009 times.
✗ Branch 2 not taken.
 400010 const Scalar & z = x[2];
78 400010 const Scalar mu_z = mu * z;
79
80
2/4
✓ Branch 1 taken 200009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200009 times.
✗ Branch 5 not taken.
 400010 const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
81
1/2
✓ Branch 1 taken 200009 times.
✗ Branch 2 not taken.
 400010 const Scalar t_norm = t.norm();
82
83
2/2
✓ Branch 0 taken 69179 times.
✓ Branch 1 taken 130830 times.
 400010 if (mu * t_norm <= -z)
84
2/4
✓ Branch 1 taken 69179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69179 times.
✗ Branch 5 not taken.
 138350 return Vector3Plain::Zero();
85
2/2
✓ Branch 0 taken 54212 times.
✓ Branch 1 taken 76618 times.
 261660 else if (t_norm <= mu_z)
86 {
87
1/2
✓ Branch 1 taken 54212 times.
✗ Branch 2 not taken.
 108424 return x;
88 }
89 else
90 {
91
1/2
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
 153236 Vector3Plain res;
92
3/6
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76618 times.
✗ Branch 8 not taken.
 153236 res.template head<2>() = (mu / t_norm) * t;
93
1/2
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
 153236 res[2] = 1;
94
1/2
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
 153236 res.normalize();
95
1/2
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
 153236 const Scalar scale = x.dot(res);
96
1/2
✓ Branch 1 taken 76618 times.
✗ Branch 2 not taken.
 153236 res *= scale;
97 153236 return res;
98 }
99 }
100
101 /// \brief Project a vector x onto the cone with a matric specified by the diagonal matrix R.
102 ///
103 /// \param[in] x a 3d vector to project.
104 /// \param[in] R a 3d vector representing the diagonal of the weights matrix. The tangential
105 /// components (the first two) of R should be equal.
106 ///
107 template<typename Vector3Like>
108 8 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) weightedProject(
109 const Eigen::MatrixBase<Vector3Like> & x, const Eigen::MatrixBase<Vector3Like> & R) const
110 {
111
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 assert(mu >= 0 && "mu must be positive");
112 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
113 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
114
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 assert(R(2) > 0 && "R(2) must be strictly positive");
115
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 Scalar weighted_mu = mu * math::sqrt(R(0) / R(2));
116 8 CoulombFrictionConeTpl weighted_cone(weighted_mu);
117
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 Vector3Plain R_sqrt = R.cwiseSqrt();
118
5/10
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
 8 Vector3Plain R_sqrt_times_x = (R_sqrt.array() * x.array()).matrix();
119
6/12
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
 8 Vector3Plain res = (weighted_cone.project(R_sqrt_times_x).array() / R_sqrt.array()).matrix();
120 16 return res;
121 }
122
123 /// \brief Compute the complementary shift associted to the Coulomb friction cone for
124 /// complementarity satisfaction in complementary problems.
125 ///
126 /// \param[in] v a dual vector.
127 ///
128 template<typename Vector3Like>
129 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
130 8 computeNormalCorrection(const Eigen::MatrixBase<Vector3Like> & v) const
131 {
132 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
133 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
134
135 8 Vector3Plain res;
136
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 8 res.template head<2>().setZero();
137
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 8 res[2] = mu * v.template head<2>().norm();
138
139 8 return res;
140 }
141
142 /// \brief Compute the radial projection associted to the Coulomb friction cone.
143 ///
144 /// \param[in] f a force vector.
145 ///
146 template<typename Vector3Like>
147 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
148 200000 computeRadialProjection(const Eigen::MatrixBase<Vector3Like> & f) const
149 {
150 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
151 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
152
153
1/2
✓ Branch 1 taken 200000 times.
✗ Branch 2 not taken.
 200000 Vector3Plain res;
154
1/2
✓ Branch 1 taken 200000 times.
✗ Branch 2 not taken.
 200000 const auto & ft = f.template head<2>();
155
1/2
✓ Branch 1 taken 200000 times.
✗ Branch 2 not taken.
 200000 const Scalar ft_norm = ft.norm();
156
157
3/6
✓ Branch 1 taken 200000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200000 times.
✗ Branch 8 not taken.
 200000 res[2] = math::max(Scalar(0), f[2]);
158
1/2
✓ Branch 1 taken 200000 times.
✗ Branch 2 not taken.
 200000 const Scalar mu_fz = mu * res[2];
159
2/2
✓ Branch 0 taken 107170 times.
✓ Branch 1 taken 92830 times.
 200000 if (ft_norm > mu_fz)
160 {
161
3/6
✓ Branch 1 taken 107170 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107170 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 107170 times.
✗ Branch 8 not taken.
 107170 res.template head<2>() = Scalar(mu_fz / ft_norm) * ft;
162 }
163 else
164
2/4
✓ Branch 1 taken 92830 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92830 times.
✗ Branch 5 not taken.
 92830 res.template head<2>() = ft;
165
166 400000 return res;
167 }
168
169 template<typename Vector3Like1, typename Vector3Like2>
170 ✗ Scalar computeContactComplementarity(
171 const Eigen::MatrixBase<Vector3Like1> & v, const Eigen::MatrixBase<Vector3Like2> & f) const
172 {
173 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like1) Vector3Plain;
174 ✗ return math::fabs(f.dot(Vector3Plain(v + computeNormalCorrection(v))));
175 }
176
177 template<typename Vector3Like1, typename Vector3Like2>
178 ✗ Scalar computeConicComplementarity(
179 const Eigen::MatrixBase<Vector3Like1> & v, const Eigen::MatrixBase<Vector3Like2> & f) const
180 {
181 ✗ return math::fabs(f.dot(v));
182 }
183
184 /// \brief Returns the dual cone associated to this.
185 1 DualCone dual() const
186 {
187 1 return DualCone(mu);
188 }
189
190 /// \brief Returns the dimension of the cone.
191 ✗ static int dim()
192 {
193 ✗ return 3;
194 }
195
196 /// \var Friction coefficient
197 Scalar mu;
198 }; // CoulombFrictionConeTpl
199
200 ///  \brief Dual of the 3d Coulomb friction cone.
201 template<typename _Scalar>
202 struct DualCoulombFrictionConeTpl
203 {
204 typedef _Scalar Scalar;
205 typedef CoulombFrictionConeTpl<Scalar> DualCone;
206
207 /// \brief Default constructor
208 ///
209 /// \param[in] mu Friction coefficient
210 1 explicit DualCoulombFrictionConeTpl(const Scalar mu)
211 1 : mu(mu)
212 {
213
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 assert(mu >= 0 && "mu must be positive");
214 1 }
215
216 /// \brief Copy constructor.
217 DualCoulombFrictionConeTpl(const DualCoulombFrictionConeTpl & other) = default;
218
219 /// \brief Copy operator
220 DualCoulombFrictionConeTpl & operator=(const DualCoulombFrictionConeTpl & other) = default;
221
222 /// \brief Comparison operator
223 ✗ bool operator==(const DualCoulombFrictionConeTpl & other) const
224 {
225 ✗ return mu == other.mu;
226 }
227
228 /// \brief Difference operator
229 ✗ bool operator!=(const DualCoulombFrictionConeTpl & other) const
230 {
231 ✗ return !(*this == other);
232 }
233
234 /// \brief Check whether a vector v lies within the cone.
235 ///
236 /// \param[in] v vector to check (assimilated to a linear velocity).
237 ///
238 template<typename Vector3Like>
239 600002 bool isInside(const Eigen::MatrixBase<Vector3Like> & v, const Scalar prec = Scalar(0)) const
240 {
241
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300001 times.
 600002 assert(mu >= 0 && "mu must be positive");
242 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
243
2/4
✓ Branch 2 taken 300001 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 300001 times.
✗ Branch 6 not taken.
 600002 return mu * v.template head<2>().norm() <= v[2] + prec;
244 }
245
246 /// \brief Project a vector x onto the cone
247 template<typename Vector3Like>
248 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
249 200001 project(const Eigen::MatrixBase<Vector3Like> & x) const
250 {
251
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 200001 assert(mu >= 0 && "mu must be positive");
252 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
253 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
254
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 200001 const Scalar & z = x[2];
255
256
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 200001 const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
257
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 200001 const Scalar t_norm = t.norm();
258
259
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 200001 if (t_norm <= -mu * z)
260
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4045 return Vector3Plain::Zero();
261
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 195956 else if (mu * t_norm <= z)
262 {
263
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 116606 return x;
264 }
265 else
266 {
267
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 79350 Vector3Plain res;
268
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 79350 res.template head<2>() = t;
269
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 79350 res[2] = mu * t_norm;
270
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 79350 res.normalize();
271
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 79350 const Scalar scale = x.dot(res);
272
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 79350 res *= scale;
273 79350 return res;
274 }
275 }
276
277 /// \brief Returns the dimension of the cone.
278 ✗ static int dim()
279 {
280 ✗ return 3;
281 }
282
283 /// \brief Returns the dual cone associated to this. ///
284 ✗ DualCone dual() const
285 {
286 ✗ return DualCone(mu);
287 }
288
289 /// \var Friction coefficient
290 Scalar mu;
291
292 }; // DualCoulombFrictionConeTpl
293
294 } // namespace pinocchio
295
296 #endif // ifndef __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
297