Line |
Branch |
Exec |
Source |
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 |
|
|
|
19 |
|
|
/// \brief Project a vector x on the vector of cones. |
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 |
|
|
|
41 |
|
|
/// \brief Project a vector x on the dual of the cones contained in the vector of cones. |
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__ |
183 |
|
|
|