GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-solver-utils.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 0 53 0.0%
Branches: 0 100 0.0%
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