pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
coulomb-friction-cone.hpp
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
12namespace pinocchio
13{
14
15 template<typename Scalar>
16 struct DualCoulombFrictionConeTpl;
17
19 template<typename _Scalar>
21 {
22 typedef _Scalar Scalar;
24 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
25
29 explicit CoulombFrictionConeTpl(const Scalar mu)
30 : mu(mu)
31 {
32 assert(mu >= 0 && "mu must be positive");
33 }
34
37
40
43 {
44 return mu == other.mu;
45 }
46
49 {
50 return !(*this == other);
51 }
52
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");
63 return f.template head<2>().norm() <= mu * f[2] + prec;
64 }
65
70 template<typename Vector3Like>
71 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
72 project(const Eigen::MatrixBase<Vector3Like> & x) const
73 {
74 assert(mu >= 0 && "mu must be positive");
76 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
77 const Scalar & z = x[2];
78 const Scalar mu_z = mu * z;
79
80 const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
81 const Scalar t_norm = t.norm();
82
83 if (mu * t_norm <= -z)
84 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
107 template<typename Vector3Like>
108 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) weightedProject(
109 const Eigen::MatrixBase<Vector3Like> & x, const Eigen::MatrixBase<Vector3Like> & R) const
110 {
111 assert(mu >= 0 && "mu must be positive");
113 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
114 assert(R(2) > 0 && "R(2) must be strictly positive");
115 Scalar weighted_mu = mu * math::sqrt(R(0) / R(2));
117 Vector3Plain R_sqrt = R.cwiseSqrt();
118 Vector3Plain R_sqrt_times_x = (R_sqrt.array() * x.array()).matrix();
119 Vector3Plain res = (weighted_cone.project(R_sqrt_times_x).array() / R_sqrt.array()).matrix();
120 return res;
121 }
122
128 template<typename Vector3Like>
129 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
130 computeNormalCorrection(const Eigen::MatrixBase<Vector3Like> & v) const
131 {
133 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3Plain;
134
135 Vector3Plain res;
136 res.template head<2>().setZero();
137 res[2] = mu * v.template head<2>().norm();
138
139 return res;
140 }
141
146 template<typename Vector3Like>
147 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
148 computeRadialProjection(const Eigen::MatrixBase<Vector3Like> & f) const
149 {
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
186 {
187 return DualCone(mu);
188 }
189
191 static int dim()
192 {
193 return 3;
194 }
195
197 Scalar mu;
198 }; // CoulombFrictionConeTpl
199
201 template<typename _Scalar>
203 {
204 typedef _Scalar Scalar;
206
211 : mu(mu)
212 {
213 assert(mu >= 0 && "mu must be positive");
214 }
215
218
221
224 {
225 return mu == other.mu;
226 }
227
230 {
231 return !(*this == other);
232 }
233
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");
243 return mu * v.template head<2>().norm() <= v[2] + prec;
244 }
245
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");
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
278 static int dim()
279 {
280 return 3;
281 }
282
285 {
286 return DualCone(mu);
287 }
288
290 Scalar mu;
291
292 }; // DualCoulombFrictionConeTpl
293
294} // namespace pinocchio
295
296#endif // ifndef __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
bool operator==(const CoulombFrictionConeTpl &other) const
Comparison operator.
CoulombFrictionConeTpl(const CoulombFrictionConeTpl &other)=default
Copy constructor.
CoulombFrictionConeTpl & operator=(const CoulombFrictionConeTpl &other)=default
Copy operator.
bool isInside(const Eigen::MatrixBase< Vector3Like > &f, const Scalar prec=Scalar(0)) const
Check whether a vector x lies within the cone.
Vector3Like computeNormalCorrection(const Eigen::MatrixBase< Vector3Like > &v) const
Compute the complementary shift associted to the Coulomb friction cone for complementarity satisfacti...
DualCone dual() const
Returns the dual cone associated to this.
Vector3Like computeRadialProjection(const Eigen::MatrixBase< Vector3Like > &f) const
Compute the radial projection associted to the Coulomb friction cone.
Vector3Like project(const Eigen::MatrixBase< Vector3Like > &x) const
Project a vector x onto the cone.
static int dim()
Returns the dimension of the cone.
Vector3Like weightedProject(const Eigen::MatrixBase< Vector3Like > &x, const Eigen::MatrixBase< Vector3Like > &R) const
Project a vector x onto the cone with a matric specified by the diagonal matrix R.
bool operator!=(const CoulombFrictionConeTpl &other) const
Difference operator.
CoulombFrictionConeTpl(const Scalar mu)
Default constructor.
bool isInside(const Eigen::MatrixBase< Vector3Like > &v, const Scalar prec=Scalar(0)) const
Check whether a vector v lies within the cone.
bool operator==(const DualCoulombFrictionConeTpl &other) const
Comparison operator.
DualCoulombFrictionConeTpl(const Scalar mu)
Default constructor.
DualCone dual() const
Returns the dual cone associated to this. ///.
Vector3Like project(const Eigen::MatrixBase< Vector3Like > &x) const
Project a vector x onto the cone.
static int dim()
Returns the dimension of the cone.
bool operator!=(const DualCoulombFrictionConeTpl &other) const
Difference operator.
DualCoulombFrictionConeTpl(const DualCoulombFrictionConeTpl &other)=default
Copy constructor.
DualCoulombFrictionConeTpl & operator=(const DualCoulombFrictionConeTpl &other)=default
Copy operator.