pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
contact-solver-utils.hpp
1 //
2 // Copyright (c) 2024 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_contact_solver_utils_hpp__
6 #define __pinocchio_algorithm_contact_solver_utils_hpp__
7 
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/comparison-operators.hpp"
10 #include "pinocchio/algorithm/constraints/coulomb-friction-cone.hpp"
11 #include "pinocchio/algorithm/delassus-operator-base.hpp"
12 
13 namespace pinocchio
14 {
15 
16  namespace internal
17  {
18 
20  template<
21  typename Scalar,
22  typename ConstraintAllocator,
23  typename VectorLikeIn,
24  typename VectorLikeOut>
25  void computeConeProjection(
26  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
27  const Eigen::DenseBase<VectorLikeIn> & x,
28  const Eigen::DenseBase<VectorLikeOut> & x_proj_)
29  {
30  assert(x.size() == x_proj_.size());
31  Eigen::DenseIndex index = 0;
32  VectorLikeOut & x_proj = x_proj_.const_cast_derived();
33  for (const auto & cone : cones)
34  {
35  x_proj.template segment<3>(index) = cone.project(x.template segment<3>(index));
36  assert(cone.isInside(x_proj.template segment<3>(index), Scalar(1e-12)));
37  index += 3;
38  }
39  }
40 
42  template<
43  typename Scalar,
44  typename ConstraintAllocator,
45  typename VectorLikeIn,
46  typename VectorLikeOut>
47  void computeDualConeProjection(
48  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
49  const Eigen::DenseBase<VectorLikeIn> & x,
50  const Eigen::DenseBase<VectorLikeOut> & x_proj_)
51  {
52  assert(x.size() == x_proj_.size());
53  Eigen::DenseIndex index = 0;
54  VectorLikeOut & x_proj = x_proj_.const_cast_derived();
55  for (const auto & cone : cones)
56  {
57  x_proj.template segment<3>(index) = cone.dual().project(x.template segment<3>(index));
58  assert(cone.dual().isInside(x_proj.template segment<3>(index), Scalar(1e-12)));
59  index += 3;
60  }
61  }
62 
63  template<
64  typename Scalar,
65  typename ConstraintAllocator,
66  typename VectorLikeVelocity,
67  typename VectorLikeForce>
68  Scalar computeConicComplementarity(
69  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
70  const Eigen::DenseBase<VectorLikeVelocity> & velocities,
71  const Eigen::DenseBase<VectorLikeForce> & forces)
72  {
73  assert(velocities.size() == forces.size());
74  Eigen::DenseIndex index = 0;
75  Scalar complementarity = 0;
76  for (const auto & cone : cones)
77  {
78  const Scalar cone_complementarity = cone.computeConicComplementarity(
79  velocities.template segment<3>(index), forces.template segment<3>(index));
80  complementarity = math::max(complementarity, cone_complementarity);
81  index += 3;
82  }
83 
84  return complementarity;
85  }
86 
87  template<
88  typename Scalar,
89  typename ConstraintAllocator,
90  typename VectorLikeIn,
91  typename VectorLikeOut>
92  void computeComplementarityShift(
93  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
94  const Eigen::DenseBase<VectorLikeIn> & velocities,
95  const Eigen::DenseBase<VectorLikeOut> & shift_)
96  {
97  assert(velocities.size() == shift_.size());
98  Eigen::DenseIndex index = 0;
99  VectorLikeOut & shift = shift_.const_cast_derived();
100  for (const auto & cone : cones)
101  {
102  shift.template segment<3>(index) =
103  cone.computeNormalCorrection(velocities.template segment<3>(index));
104  index += 3;
105  }
106  }
107 
108  template<typename Scalar, typename ConstraintAllocator, typename VectorLikeIn>
109  Scalar computePrimalFeasibility(
110  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
111  const Eigen::DenseBase<VectorLikeIn> & forces)
112  {
113  typedef CoulombFrictionConeTpl<Scalar> Cone;
114  typedef typename Cone::Vector3 Vector3;
115 
116  Eigen::DenseIndex index = 0;
117  Scalar norm(0);
118  for (const auto & cone : cones)
119  {
120  const Vector3 df_projected =
121  cone.project(forces.template segment<3>(index)) - forces.template segment<3>(index);
122  norm = math::max(norm, df_projected.norm());
123  index += 3;
124  }
125 
126  return norm;
127  }
128 
129  template<
130  typename Scalar,
131  typename ConstraintAllocator,
132  typename ForceVector,
133  typename VelocityVector>
134  Scalar computeReprojectionError(
135  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
136  const Eigen::DenseBase<ForceVector> & forces,
137  const Eigen::DenseBase<VelocityVector> & velocities)
138  {
139  typedef CoulombFrictionConeTpl<Scalar> Cone;
140  typedef typename Cone::Vector3 Vector3;
141 
142  Eigen::DenseIndex index = 0;
143  Scalar norm(0);
144  for (const auto & cone : cones)
145  {
146  const Vector3 df_projected =
147  forces.template segment<3>(index)
148  - cone.project(forces.template segment<3>(index) - velocities.template segment<3>(index));
149  norm = math::max(norm, df_projected.norm());
150  index += 3;
151  }
152 
153  return norm;
154  }
155 
156  // template<typename Scalar, typename ConstraintAllocator, typename VectorLikeIn>
157  // Scalar computeDualFeasibility(DelassusOperatorBase<DelassusDerived> & delassus,
158  // const Eigen::MatrixBase<VectorLike> & g,
159  // const
160  // std::vector<CoulombFrictionConeTpl<Scalar>,ConstraintAllocator>
161  // & cones, const Eigen::DenseBase<VectorLikeIn> & forces)
162  //{
163  // typedef CoulombFrictionConeTpl<Scalar> Cone;
164  // typedef typename Cone::Vector3 Vector3;
165  //
166  // Eigen::DenseIndex index = 0;
167  // Scalar norm = 0;
168  // for(const auto & cone: cones)
169  // {
170  // const Vector3 df_projected = cone.project(forces.template segment<3>(index)) -
171  // forces.template segment<3>(index); norm = math::max(complementarity,
172  // df_projected.norm()); index += 3;
173  // }
174  //
175  // return norm;
176  // }
177 
178  } // namespace internal
179 
180 } // namespace pinocchio
181 
182 #endif // ifndef __pinocchio_algorithm_contact_solver_utils_hpp__
pinocchio::internal::computeDualConeProjection
void computeDualConeProjection(const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &x, const Eigen::DenseBase< VectorLikeOut > &x_proj_)
Project a vector x on the dual of the cones contained in the vector of cones.
Definition: contact-solver-utils.hpp:47
pinocchio::internal::computeConeProjection
void computeConeProjection(const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &x, const Eigen::DenseBase< VectorLikeOut > &x_proj_)
Project a vector x on the vector of cones.
Definition: contact-solver-utils.hpp:25
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11