Directory: | ./ |
---|---|
File: | src/weighed-distance.cc |
Date: | 2024-08-10 11:29:48 |
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 | 68646 | 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 33369 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 955 times.
✓ Branch 6 taken 33369 times.
✗ Branch 7 not taken.
|
137294 | distance = LG_t().squaredDistance(jmodel.jointConfigSelector(q0), |
150 |
1/2✓ Branch 1 taken 34324 times.
✗ Branch 2 not taken.
|
68646 | 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 | 16448 | value_type WeighedDistance::impl_distance(ConfigurationIn_t q1, | |
273 | ConfigurationIn_t q2) const { | ||
274 | 16448 | value_type res = 0, d = std::numeric_limits<value_type>::infinity(); | |
275 | |||
276 | 16448 | const pinocchio::Model& model = robot_->model(); | |
277 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 16448 times.
|
16448 | 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 34324 times.
✓ Branch 1 taken 16448 times.
|
50772 | for (pinocchio::JointIndex i = 1; i < (pinocchio::JointIndex)model.njoints; |
281 | ++i) { | ||
282 |
2/4✓ Branch 1 taken 34323 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34323 times.
✗ Branch 5 not taken.
|
34324 | value_type length = weights_[i - 1] * weights_[i - 1]; |
283 |
1/2✓ Branch 1 taken 34324 times.
✗ Branch 2 not taken.
|
34323 | SquaredDistanceStep::ArgsType args(q1, q2, length, d); |
284 |
2/4✓ Branch 1 taken 34323 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 34324 times.
✗ Branch 6 not taken.
|
34324 | SquaredDistanceStep::run(model.joints[i], args); |
285 | 34324 | res += d; | |
286 | 34324 | } | |
287 |
3/6✓ Branch 1 taken 16448 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 16448 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16448 times.
✗ Branch 11 not taken.
|
16448 | res += (q1 - q2).tail(robot_->extraConfigSpace().dimension()).squaredNorm(); |
288 | 32896 | return sqrt(res); | |
289 | } | ||
290 | } // namespace core | ||
291 | } // namespace hpp | ||
292 |