GCC Code Coverage Report

Directory: ./
File: src/weighed-distance.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 86 122 70.5%
Branches: 84 200 42.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <Eigen/SVD>
31 #include <hpp/core/problem.hh>
32 #include <hpp/core/weighed-distance.hh>
33 #include <hpp/pinocchio/device.hh>
34 #include <hpp/pinocchio/joint-collection.hh>
35 #include <hpp/pinocchio/joint.hh>
36 #include <hpp/pinocchio/liegroup.hh>
37 #include <hpp/pinocchio/util.hh>
38 #include <hpp/util/debug.hh>
39 #include <limits>
40 #include <pinocchio/algorithm/jacobian.hpp>
41 #include <pinocchio/algorithm/joint-configuration.hpp>
42 #include <pinocchio/multibody/geometry.hpp>
43
44 namespace hpp {
45 namespace core {
46 namespace {
47 struct ComputeWeightStep
48 : public ::pinocchio::fusion::JointUnaryVisitorBase<ComputeWeightStep> {
49 typedef boost::fusion::vector<const pinocchio::Model&, const pinocchio::Data&,
50 const pinocchio::GeomData&, value_type&>
51 ArgsType;
52
53 template <int N>
54 290 static value_type largestSingularValue(
55 const Eigen::Matrix<value_type, 3, N>& m) {
56 typedef Eigen::Matrix<value_type, 3, N> M;
57 typedef Eigen::JacobiSVD<M> SVD_t;
58
1/2
✓ Branch 1 taken 290 times.
✗ Branch 2 not taken.
 290 SVD_t svd(m);
59
2/4
✓ Branch 1 taken 290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 290 times.
✗ Branch 5 not taken.
 580 return svd.singularValues()[0];
60 290 }
61
62 template <typename JointModel, int NR>
63 2322 static void getRotationSubJacobian(
64 const ::pinocchio::JointModelBase<JointModel>& jmodel,
65 const pinocchio::Data& data,
66 const Eigen::Matrix<value_type, 6, Eigen::Dynamic>& J,
67 const pinocchio::JointIndex& j,
68 Eigen::Matrix<value_type, 6, NR>& rBlock) {
69 typedef typename pinocchio::RnxSOnLieGroupMap::template operation<
70 JointModel>::type LGOp_t;
71 typedef Eigen::Matrix<value_type, 6, JointModel::NV> Block_t;
72
1/2
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
 2322 Block_t block;
73 // Linear part
74
2/4
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
 2322 block.template topRows<3>() =
75
4/8
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1161 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1161 times.
✗ Branch 12 not taken.
 2322 data.oMi[j].rotation() * J.block<3, JointModel::NV>(0, jmodel.idx_v());
76 // Angular part
77
2/4
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
 2322 block.template bottomRows<3>() =
78
4/8
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1161 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1161 times.
✗ Branch 12 not taken.
 2322 data.oMi[j].rotation() * J.block<3, JointModel::NV>(3, jmodel.idx_v());
79
80
1/2
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
 2322 LGOp_t::getRotationSubJacobian(block, rBlock);
81 }
82
83 template <int NR>
84 2322 static value_type computeWeight(
85 const Eigen::Matrix<value_type, 6, NR>& rBlock, const value_type& r) {
86 2322 value_type linear_sigma =
87
3/6
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1161 times.
✗ Branch 8 not taken.
 2322 largestSingularValue<NR>(rBlock.template topRows<3>());
88 value_type angular_sigma =
89
3/6
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1161 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1161 times.
✗ Branch 8 not taken.
 2322 largestSingularValue<NR>(rBlock.template bottomRows<3>());
90
91 2322 return std::max(linear_sigma, r * angular_sigma);
92 }
93
94 template <typename JointModel>
95 608 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
96 const pinocchio::Model& model, const pinocchio::Data& data,
97 const pinocchio::GeomData& geomData, value_type& length) {
98 typedef typename pinocchio::RnxSOnLieGroupMap::template operation<
99 JointModel>::type LGOp_t;
100 typedef Eigen::Matrix<value_type, 6, LGOp_t::NR> RBlock_t;
101
102 if (LGOp_t::NR == 0) {
103 116 length = 1;
104 116 return;
105 }
106
107
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 492 const pinocchio::JointIndex i = jmodel.id();
108
1/2
✓ Branch 2 taken 246 times.
✗ Branch 3 not taken.
 492 JointJacobian_t jacobian(6, data.J.cols());
109
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 492 RBlock_t rBlock;
110 value_type sigma;
111 2814 for (pinocchio::JointIndex j = i;
112
2/2
✓ Branch 1 taken 1161 times.
✓ Branch 2 taken 246 times.
 2814 j <= (pinocchio::JointIndex)data.lastChild[i]; ++j) {
113
1/2
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
 2322 ::pinocchio::getJointJacobian(model, data, j, ::pinocchio::LOCAL,
114 jacobian);
115
1/2
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
 2322 getRotationSubJacobian(jmodel, data, jacobian, j, rBlock);
116 2322 const value_type radius =
117 2322 geomData.radius[j] +
118
4/8
✓ Branch 2 taken 1161 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1161 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1161 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1161 times.
✗ Branch 13 not taken.
 2322 (data.oMi[j].translation() - data.oMi[i].translation()).squaredNorm();
119
1/2
✓ Branch 1 taken 1161 times.
✗ Branch 2 not taken.
 2322 sigma = computeWeight(rBlock, radius);
120
2/2
✓ Branch 0 taken 640 times.
✓ Branch 1 taken 521 times.
 2322 if (length < sigma) length = sigma;
121 }
122 }
123 };
124
125 template <>
126 ✗ value_type ComputeWeightStep::largestSingularValue<0>(
127 const Eigen::Matrix<value_type, 3, 0>&) {
128 ✗ return 0;
129 }
130
131 template <>
132 2032 value_type ComputeWeightStep::largestSingularValue<1>(
133 const Eigen::Matrix<value_type, 3, 1>& m) {
134 2032 return m.norm();
135 }
136
137 struct SquaredDistanceStep
138 : public ::pinocchio::fusion::JointUnaryVisitorBase<SquaredDistanceStep> {
139 typedef boost::fusion::vector<ConfigurationIn_t, ConfigurationIn_t,
140 const value_type&, value_type&>
141 ArgsType;
142
143 template <typename JointModel>
144 68710 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
145 ConfigurationIn_t q0, ConfigurationIn_t q1,
146 const value_type& w, value_type& distance) {
147 typedef typename ::hpp::pinocchio::RnxSOnLieGroupMap::template operation<
148 JointModel>::type LG_t;
149
4/6
✓ Branch 2 taken 955 times.
✓ Branch 3 taken 33401 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 955 times.
✓ Branch 6 taken 33399 times.
✗ Branch 7 not taken.
 137422 distance = LG_t().squaredDistance(jmodel.jointConfigSelector(q0),
150
1/2
✓ Branch 1 taken 34356 times.
✗ Branch 2 not taken.
 68716 jmodel.jointConfigSelector(q1), w);
151 }
152 };
153 } // namespace
154
155 48 WeighedDistancePtr_t WeighedDistance::create(const DevicePtr_t& robot) {
156
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 48 WeighedDistance* ptr = new WeighedDistance(robot);
157 48 WeighedDistancePtr_t shPtr(ptr);
158
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 48 ptr->init(shPtr);
159 48 return shPtr;
160 }
161
162 ✗ WeighedDistancePtr_t WeighedDistance::createFromProblem(
163 const ProblemConstPtr_t& problem) {
164 ✗ WeighedDistance* ptr = new WeighedDistance(problem);
165 ✗ WeighedDistancePtr_t shPtr(ptr);
166 ✗ ptr->init(shPtr);
167 ✗ return shPtr;
168 }
169
170 3 WeighedDistancePtr_t WeighedDistance::createWithWeight(
171 const DevicePtr_t& robot, const vector_t& weights) {
172
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 WeighedDistance* ptr = new WeighedDistance(robot, weights);
173 3 WeighedDistancePtr_t shPtr(ptr);
174
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ptr->init(shPtr);
175 3 return shPtr;
176 }
177
178 ✗ WeighedDistancePtr_t WeighedDistance::createCopy(
179 const WeighedDistancePtr_t& distance) {
180 ✗ WeighedDistance* ptr = new WeighedDistance(*distance);
181 ✗ WeighedDistancePtr_t shPtr(ptr);
182 ✗ ptr->init(shPtr);
183 ✗ return shPtr;
184 }
185
186 ✗ DistancePtr_t WeighedDistance::clone() const {
187 ✗ return createCopy(weak_.lock());
188 }
189
190 ✗ const vector_t& WeighedDistance::weights() const { return weights_; }
191
192 ✗ void WeighedDistance::weights(const vector_t& ws) {
193 ✗ if (ws.size() == weights_.size()) {
194 ✗ weights_ = ws;
195 } else {
196 ✗ std::ostringstream oss;
197 ✗ oss << "Distance::weights : size mismatch. Got " << ws.size()
198 ✗ << ". Expected " << weights_.size() << ".";
199 ✗ throw std::runtime_error(oss.str());
200 }
201 }
202
203 ✗ value_type WeighedDistance::getWeight(size_type rank) const {
204 ✗ return weights_[rank];
205 }
206
207 ✗ void WeighedDistance::setWeight(size_type rank, value_type weight) {
208 ✗ if (rank < weights_.size()) {
209 ✗ weights_[rank] = weight;
210 } else {
211 ✗ std::ostringstream oss;
212 ✗ oss << "Distance::setWeight : rank " << rank << " is out of range ("
213 ✗ << weights_.size() << ").";
214 ✗ throw std::runtime_error(oss.str());
215 }
216 }
217
218 48 void WeighedDistance::computeWeights() {
219 // TODO this test is necessary because of
220 // https://github.com/stack-of-tasks/pinocchio/issues/1011
221 // It can be removed when the issue is solved.
222
3/4
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 46 times.
 48 if (robot_->configSize() == 0) {
223
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 weights_.resize(0);
224 2 return;
225 }
226
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 pinocchio::DeviceSync device(robot_);
227
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 device.computeForwardKinematics(pinocchio::JOINT_POSITION |
228 pinocchio::JACOBIAN);
229 46 value_type minLength = std::numeric_limits<value_type>::infinity();
230
231 46 const pinocchio::Model& model = device.model();
232
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 const pinocchio::Data& data = device.data();
233
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 const pinocchio::GeomData& geomData = device.geomData();
234
1/2
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
 46 weights_.resize(model.joints.size() - 1);
235 // TODO when there is only one freeflyer, and the body radius is 0,
236 // the weights should be [0,].
237 // The algorithm below returns [inf,]
238
2/2
✓ Branch 1 taken 304 times.
✓ Branch 2 taken 46 times.
 350 for (pinocchio::JointIndex i = 1; i < model.joints.size(); ++i) {
239 304 value_type length = 0;
240
2/4
✓ Branch 1 taken 304 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 304 times.
✗ Branch 6 not taken.
 304 ComputeWeightStep::run(model.joints[i], ComputeWeightStep::ArgsType(
241 model, data, geomData, length));
242
4/4
✓ Branch 0 taken 102 times.
✓ Branch 1 taken 202 times.
✓ Branch 2 taken 90 times.
✓ Branch 3 taken 12 times.
 304 if (minLength > length && length > 0) minLength = length;
243
1/2
✓ Branch 1 taken 304 times.
✗ Branch 2 not taken.
 304 weights_[i - 1] = length;
244
2/2
✓ Branch 0 taken 3144 times.
✓ Branch 1 taken 304 times.
 3448 for (std::size_t k = 0; k < i; ++k) {
245
3/4
✓ Branch 1 taken 3144 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✓ Branch 4 taken 3132 times.
 3144 if (weights_[k] == 0) {
246
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 weights_[k] = minLength;
247 }
248 }
249 }
250 hppDout(info, "The weights are " << weights_.transpose());
251 46 }
252
253 48 WeighedDistance::WeighedDistance(const DevicePtr_t& robot)
254
1/2
✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
 48 : robot_(robot), weights_() {
255
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 computeWeights();
256 48 }
257
258 ✗ WeighedDistance::WeighedDistance(const ProblemConstPtr_t& problem)
259 ✗ : robot_(problem->robot()), weights_() {
260 ✗ computeWeights();
261 }
262
263 3 WeighedDistance::WeighedDistance(const DevicePtr_t& robot,
264 3 const vector_t& weights)
265
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 : robot_(robot), weights_(weights) {}
266
267 ✗ WeighedDistance::WeighedDistance(const WeighedDistance& distance)
268 ✗ : robot_(distance.robot_), weights_(distance.weights_) {}
269
270 51 void WeighedDistance::init(WeighedDistanceWkPtr_t self) { weak_ = self; }
271
272 16465 value_type WeighedDistance::impl_distance(ConfigurationIn_t q1,
273 ConfigurationIn_t q2) const {
274 16465 value_type res = 0, d = std::numeric_limits<value_type>::infinity();
275
276 16465 const pinocchio::Model& model = robot_->model();
277
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16465 times.
 16465 assert((size_type)model.joints.size() <= weights_.size() + 1);
278 // Configuration_t qq1 (q1), qq2 (q2);
279 // Loop over robot joint
280
2/2
✓ Branch 0 taken 34357 times.
✓ Branch 1 taken 16464 times.
 50821 for (pinocchio::JointIndex i = 1; i < (pinocchio::JointIndex)model.njoints;
281 ++i) {
282
2/4
✓ Branch 1 taken 34358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34358 times.
✗ Branch 5 not taken.
 34357 value_type length = weights_[i - 1] * weights_[i - 1];
283
1/2
✓ Branch 1 taken 34356 times.
✗ Branch 2 not taken.
 34358 SquaredDistanceStep::ArgsType args(q1, q2, length, d);
284
2/4
✓ Branch 1 taken 34353 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 34357 times.
✗ Branch 6 not taken.
 34356 SquaredDistanceStep::run(model.joints[i], args);
285 34358 res += d;
286 34358 }
287
3/6
✓ Branch 1 taken 16465 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 16465 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16465 times.
✗ Branch 11 not taken.
 16464 res += (q1 - q2).tail(robot_->extraConfigSpace().dimension()).squaredNorm();
288 32930 return sqrt(res);
289 }
290 } // namespace core
291 } // namespace hpp
292