pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
13namespace 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::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::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
Main pinocchio namespace.
Definition treeview.dox:11