GCC Code Coverage Report


Directory: ./
File: src/path-optimization/partial-shortcut.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 161 0.0%
Branches: 0 278 0.0%

Line Branch Exec Source
1 // Copyright (c) 2015, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/constraints/locked-joint.hh>
30 #include <hpp/core/config-projector.hh>
31 #include <hpp/core/distance.hh>
32 #include <hpp/core/path-optimization/partial-shortcut.hh>
33 #include <hpp/core/path-validation.hh>
34 #include <hpp/core/path-vector.hh>
35 #include <hpp/core/problem.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/joint-collection.hh>
38 #include <hpp/pinocchio/joint.hh>
39 #include <hpp/pinocchio/liegroup.hh>
40 #include <hpp/util/debug.hh>
41 #include <pinocchio/algorithm/joint-configuration.hpp>
42
43 namespace hpp {
44 namespace core {
45 namespace pathOptimization {
46 namespace {
47 void unpack(PathPtr_t path, PathVectorPtr_t out) {
48 PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path);
49 if (!pv) {
50 out->appendPath(path);
51 } else {
52 for (std::size_t i = 0; i < pv->numberPaths(); ++i)
53 unpack(pv->pathAtRank(i), out);
54 }
55 }
56
57 // Compute the length of a vector of paths assuming that each element
58 // is optimal for the given distance.
59 static value_type pathLength(const PathVectorPtr_t& path,
60 const DistancePtr_t& distance) {
61 value_type result = 0;
62 for (std::size_t i = 0; i < path->numberPaths(); ++i) {
63 const PathPtr_t& element(path->pathAtRank(i));
64 result += (*distance)(element->initial(), element->end());
65 }
66 return result;
67 }
68 } // namespace
69
70 PartialShortcut::Parameters::Parameters()
71 : removeLockedJoints(true),
72 onlyFullShortcut(true),
73 numberOfConsecutiveFailurePerJoints(5),
74 progressionMargin(1e-3) {}
75
76 PartialShortcutPtr_t PartialShortcut::create(const ProblemConstPtr_t& problem) {
77 return createWithTraits<PartialShortcutTraits>(problem);
78 }
79
80 PartialShortcut::PartialShortcut(const ProblemConstPtr_t& problem)
81 : PathOptimizer(problem) {}
82
83 PathVectorPtr_t PartialShortcut::optimize(const PathVectorPtr_t& path) {
84 PathVectorPtr_t unpacked =
85 PathVector::create(path->outputSize(), path->outputDerivativeSize());
86 unpack(path, unpacked);
87
88 /// Step 1: Generate a suitable vector of joints
89 JointStdVector_t straight_jv = generateJointVector(unpacked);
90 JointStdVector_t jv;
91
92 /// Step 2: First try to optimize each joint from beginning to end
93 PathVectorPtr_t result = optimizeFullPath(unpacked, straight_jv, jv);
94 if (parameters.onlyFullShortcut) return result;
95
96 /// Step 3: Optimize randomly each joint
97 return optimizeRandom(result, jv);
98 }
99
100 PathVectorPtr_t PartialShortcut::generatePath(
101 PathVectorPtr_t path, JointConstPtr_t joint, const value_type t1,
102 ConfigurationIn_t q1, const value_type t2, ConfigurationIn_t q2) const {
103 value_type lt1, lt2;
104 // TODO: correct API so thatn these casts are no longer necessary!!
105 JointPtr_t j = const_pointer_cast<pinocchio::Joint>(joint);
106 std::size_t rkAtP1 = path->rankAtParam(t1, lt1);
107 std::size_t rkAtP2 = path->rankAtParam(t2, lt2);
108 if (rkAtP2 == rkAtP1) return PathVectorPtr_t();
109
110 PathVectorPtr_t pv =
111 PathVector::create(path->outputSize(), path->outputDerivativeSize());
112 PathPtr_t last;
113
114 Configuration_t qi = q1;
115 Configuration_t q_inter(path->outputSize());
116 value_type t = -lt1;
117 for (std::size_t i = rkAtP1; i < rkAtP2; ++i) {
118 t += path->pathAtRank(i)->length();
119 q_inter = path->pathAtRank(i)->end();
120 // q_inter.segment(rkCfg,szCfg) = j->jointModel().interpolate ( q1, q2,
121 // t / (t2-t1));
122 typedef pinocchio::RnxSOnLieGroupMap LG_t;
123 typedef ::pinocchio::InterpolateStep<
124 LG_t, ConfigurationIn_t, ConfigurationIn_t, value_type, Configuration_t>
125 IS_t;
126 value_type u = t / (t2 - t1);
127 IS_t::run(j->jointModel(), IS_t::ArgsType(q1, q2, u, q_inter));
128 if (path->pathAtRank(i)->constraints()) {
129 if (!path->pathAtRank(i)->constraints()->apply(q_inter)) {
130 hppDout(warning,
131 "PartialShortcut could not apply "
132 "the constraints");
133 return PathVectorPtr_t();
134 }
135 }
136 last = (steer)(qi, q_inter);
137 if (!last) return PathVectorPtr_t();
138 pv->appendPath(last);
139 qi = q_inter;
140 }
141 last = steer(qi, q2);
142 if (!last) return PathVectorPtr_t();
143 pv->appendPath(last);
144 PathVectorPtr_t out =
145 PathVector::create(path->outputSize(), path->outputDerivativeSize());
146 pv->flatten(out);
147 return out;
148 }
149
150 JointStdVector_t PartialShortcut::generateJointVector(
151 const PathVectorPtr_t& pv) const {
152 DevicePtr_t robot = problem()->robot();
153
154 JointStdVector_t jv;
155 ConfigProjectorPtr_t proj =
156 pv->pathAtRank(0)->constraints()->configProjector();
157 NumericalConstraints_t constraints;
158 if (proj) constraints = proj->numericalConstraints();
159
160 for (size_type iJ = 0; iJ < robot->nbJoints(); ++iJ) {
161 JointPtr_t joint = robot->jointAt(iJ);
162 // TODO this test is always true.
163 if (joint->numberDof() > 0) {
164 bool lock = false;
165 if (parameters.removeLockedJoints && proj) {
166 const size_type rkCfg = joint->rankInConfiguration();
167 for (NumericalConstraints_t::const_iterator it(constraints.begin());
168 it != constraints.end(); ++it) {
169 LockedJointPtr_t lj(HPP_DYNAMIC_PTR_CAST(LockedJoint, *it));
170 if (lj && lj->rankInConfiguration() == rkCfg) {
171 lock = true;
172 break;
173 }
174 }
175 }
176 if (!lock) jv.push_back(joint);
177 }
178 }
179 return jv;
180 }
181
182 PathVectorPtr_t PartialShortcut::optimizeFullPath(
183 const PathVectorPtr_t& pv, const JointStdVector_t& jvIn,
184 JointStdVector_t& jvOut) const {
185 Configuration_t q0 = pv->initial();
186 Configuration_t q3 = pv->end();
187 const value_type t0 = 0;
188 value_type t3;
189 PathVectorPtr_t opted = pv;
190
191 /// First try to optimize each joint from beginning to end
192 for (std::size_t iJ = 0; iJ < jvIn.size(); ++iJ) {
193 t3 = opted->timeRange().second;
194 JointConstPtr_t joint = jvIn.at(iJ);
195
196 // Validate sub parts
197 bool valid;
198 PathVectorPtr_t straight;
199 straight = generatePath(opted, joint, t0, q0, t3, q3);
200 {
201 PathPtr_t validPart;
202 PathValidationReportPtr_t report;
203 if (!straight)
204 valid = false;
205 else {
206 valid = problem()->pathValidation()->validate(straight, false,
207 validPart, report);
208 }
209 }
210 if (!valid) {
211 jvOut.push_back(joint);
212 continue;
213 }
214 opted = straight;
215
216 hppDout(info, "length = " << pathLength(opted, problem()->distance())
217 << ", joint " << joint->name());
218 }
219 return opted;
220 }
221
222 PathVectorPtr_t PartialShortcut::optimizeRandom(
223 const PathVectorPtr_t& pv, const JointStdVector_t& jv) const {
224 PathVectorPtr_t current = pv, result = pv;
225 const value_type t0 = 0;
226 value_type t3;
227 Configuration_t q0 = pv->initial();
228 Configuration_t q3 = pv->end();
229 value_type length = pathLength(pv, problem()->distance()),
230 newLength = std::numeric_limits<value_type>::infinity();
231
232 hppDout(info, "random partial shorcut on " << jv.size() << " joints.");
233
234 // Maximal number of iterations without improvements
235 const std::size_t maxFailure =
236 jv.size() * parameters.numberOfConsecutiveFailurePerJoints;
237 std::size_t nbFail = 0;
238 std::size_t iJ = 0;
239 Configuration_t q1(pv->outputSize()), q2(pv->outputSize());
240 while (nbFail < maxFailure) {
241 iJ %= jv.size();
242 JointConstPtr_t joint = jv.at(iJ);
243 ++iJ;
244
245 t3 = current->timeRange().second;
246 value_type u2 = t3 * rand() / RAND_MAX;
247 value_type u1 = t3 * rand() / RAND_MAX;
248
249 value_type t1, t2;
250 if (u1 < u2) {
251 t1 = u1;
252 t2 = u2;
253 } else {
254 t1 = u2;
255 t2 = u1;
256 }
257 bool success = current->eval(q1, t1) && current->eval(q2, t2);
258 if (success) {
259 hppDout(warning,
260 "The constraints could not be applied to the "
261 "current path");
262 nbFail++;
263 continue;
264 }
265 // Validate sub parts
266 bool valid[3];
267 PathVectorPtr_t straight[3];
268 straight[0] = generatePath(current, joint, t0, q0, t1, q1);
269 straight[1] = generatePath(current, joint, t1, q1, t2, q2);
270 straight[2] = generatePath(current, joint, t2, q2, t3, q3);
271 for (unsigned i = 0; i < 3; ++i) {
272 PathPtr_t validPart;
273 PathValidationReportPtr_t report;
274 if (!straight[i])
275 valid[i] = false;
276 else {
277 valid[i] = problem()->pathValidation()->validate(straight[i], false,
278 validPart, report);
279 }
280 }
281 if (!valid[0] && !valid[1] && !valid[2]) {
282 nbFail++;
283 continue;
284 }
285 // Replace valid parts
286 result = PathVector::create(pv->outputSize(), pv->outputDerivativeSize());
287 if (valid[0])
288 result->concatenate(straight[0]);
289 else
290 result->concatenate(
291 (current->extract(std::make_pair(t0, t1))->as<PathVector>()));
292 if (valid[1])
293 result->concatenate(straight[1]);
294 else
295 result->concatenate(
296 (current->extract(std::make_pair(t1, t2))->as<PathVector>()));
297 if (valid[2])
298 result->concatenate(straight[2]);
299 else
300 result->concatenate(
301 current->extract(std::make_pair(t2, t3))->as<PathVector>());
302
303 newLength = pathLength(result, problem()->distance());
304 if (newLength >= length) {
305 nbFail++;
306 continue;
307 }
308 if (newLength >= length - parameters.progressionMargin)
309 nbFail++;
310 else
311 nbFail = 0;
312 --iJ; // This joint could be optimized. Try another time on it.
313 length = newLength;
314 hppDout(info, "length = " << length << ", nbFail = " << nbFail << ", joint "
315 << joint->name());
316 current = result;
317 }
318 return result;
319 }
320 } // namespace pathOptimization
321 } // namespace core
322 } // namespace hpp
323