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 |