GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constraints/coulomb-friction-cone.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 26 91 28.6%
Branches: 30 162 18.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 4 explicit CoulombFrictionConeTpl(const Scalar mu)
30 4 : mu(mu)
31 {
32
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 assert(mu >= 0 && "mu must be positive");
33 4 }
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 ✗ bool isInside(const Eigen::MatrixBase<Vector3Like> & f, const Scalar prec = Scalar(0)) const
59 {
60 ✗ assert(mu >= 0 && "mu must be positive");
61 ✗ assert(prec >= 0 && "prec should be positive");
62 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
63 ✗ 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 2 project(const Eigen::MatrixBase<Vector3Like> & x) const
73 {
74
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 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 2 times.
✗ Branch 2 not taken.
 2 const Scalar & z = x[2];
78 2 const Scalar mu_z = mu * z;
79
80
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
81
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const Scalar t_norm = t.norm();
82
83
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (mu * t_norm <= -z)
84
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 return Vector3Plain::Zero();
85 ✗ else if (t_norm <= mu_z)
86 {
87 ✗ return x;
88 }
89 else
90 {
91 ✗ Vector3Plain res;
92 ✗ res.template head<2>() = (mu / t_norm) * t;
93 ✗ res[2] = 1;
94 ✗ res.normalize();
95 ✗ const Scalar scale = x.dot(res);
96 ✗ res *= scale;
97 ✗ 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 2 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 2 times.
 2 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 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(R(2) > 0 && "R(2) must be strictly positive");
115
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar weighted_mu = mu * math::sqrt(R(0) / R(2));
116 2 CoulombFrictionConeTpl weighted_cone(weighted_mu);
117
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Vector3Plain R_sqrt = R.cwiseSqrt();
118
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 Vector3Plain R_sqrt_times_x = (R_sqrt.array() * x.array()).matrix();
119
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 Vector3Plain res = (weighted_cone.project(R_sqrt_times_x).array() / R_sqrt.array()).matrix();
120 4 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 2 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 2 Vector3Plain res;
136
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 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.
 2 res[2] = mu * v.template head<2>().norm();
138
139 2 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 ✗ 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 ✗ Vector3Plain res;
154 ✗ const auto & ft = f.template head<2>();
155 ✗ const Scalar ft_norm = ft.norm();
156
157 ✗ res[2] = math::max(Scalar(0), f[2]);
158 ✗ const Scalar mu_fz = mu * res[2];
159 ✗ if (ft_norm > mu_fz)
160 {
161 ✗ res.template head<2>() = Scalar(mu_fz / ft_norm) * ft;
162 }
163 else
164 ✗ res.template head<2>() = ft;
165
166 ✗ 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 ✗ DualCone dual() const
186 {
187 ✗ 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 ✗ explicit DualCoulombFrictionConeTpl(const Scalar mu)
211 ✗ : mu(mu)
212 {
213 ✗ assert(mu >= 0 && "mu must be positive");
214 }
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 ✗ bool isInside(const Eigen::MatrixBase<Vector3Like> & v, const Scalar prec = Scalar(0)) const
240 {
241 ✗ assert(mu >= 0 && "mu must be positive");
242 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
243 ✗ 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 ✗ project(const Eigen::MatrixBase<Vector3Like> & x) const
250 {
251 ✗ 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 ✗ const Scalar & z = x[2];
255
256 ✗ const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
257 ✗ const Scalar t_norm = t.norm();
258
259 ✗ if (t_norm <= -mu * z)
260 ✗ return Vector3Plain::Zero();
261 ✗ else if (mu * t_norm <= z)
262 {
263 ✗ return x;
264 }
265 else
266 {
267 ✗ Vector3Plain res;
268 ✗ res.template head<2>() = t;
269 ✗ res[2] = mu * t_norm;
270 ✗ res.normalize();
271 ✗ const Scalar scale = x.dot(res);
272 ✗ res *= scale;
273 ✗ 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