5#ifndef __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
6#define __pinocchio_algorithm_constraints_coulomb_friction_cone_hpp__
8#include "pinocchio/algorithm/constraints/fwd.hpp"
9#include "pinocchio/math/fwd.hpp"
10#include "pinocchio/math/comparison-operators.hpp"
15 template<
typename Scalar>
16 struct DualCoulombFrictionConeTpl;
19 template<
typename _Scalar>
24 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
32 assert(mu >= 0 &&
"mu must be positive");
44 return mu ==
other.mu;
50 return !(*
this ==
other);
57 template<
typename Vector3Like>
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;
70 template<
typename Vector3Like>
71 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
74 assert(mu >= 0 &&
"mu must be positive");
76 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
Vector3Plain;
80 const Eigen::Matrix<Scalar, 2, 1>
t = x.template
head<2>();
84 return Vector3Plain::Zero();
95 const Scalar scale = x.dot(res);
107 template<
typename Vector3Like>
109 const Eigen::MatrixBase<Vector3Like> & x,
const Eigen::MatrixBase<Vector3Like> & R)
const
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");
128 template<
typename Vector3Like>
129 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
133 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
Vector3Plain;
136 res.template
head<2>().setZero();
137 res[2] = mu * v.template
head<2>().norm();
146 template<
typename Vector3Like>
147 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
151 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
Vector3Plain;
157 res[2] = math::max(
Scalar(0), f[2]);
169 template<
typename Vector3Like1,
typename Vector3Like2>
170 Scalar computeContactComplementarity(
171 const Eigen::MatrixBase<Vector3Like1> & v,
const Eigen::MatrixBase<Vector3Like2> & f)
const
177 template<
typename Vector3Like1,
typename Vector3Like2>
178 Scalar computeConicComplementarity(
179 const Eigen::MatrixBase<Vector3Like1> & v,
const Eigen::MatrixBase<Vector3Like2> & f)
const
181 return math::fabs(f.dot(v));
201 template<
typename _Scalar>
213 assert(mu >= 0 &&
"mu must be positive");
225 return mu ==
other.mu;
231 return !(*
this ==
other);
238 template<
typename Vector3Like>
241 assert(mu >= 0 &&
"mu must be positive");
243 return mu * v.template
head<2>().norm() <= v[2] +
prec;
247 template<
typename Vector3Like>
248 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
251 assert(mu >= 0 &&
"mu must be positive");
253 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)
Vector3Plain;
256 const Eigen::Matrix<Scalar, 2, 1>
t = x.template
head<2>();
260 return Vector3Plain::Zero();
271 const Scalar scale = x.dot(res);
Main pinocchio namespace.
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.