Directory: | ./ |
---|---|
File: | src/problem.impl.cc |
Date: | 2024-12-13 15:50:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 67 | 1399 | 4.8% |
Branches: | 49 | 3950 | 1.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, Joseph Mirabel, | ||
2 | // JRL. | ||
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 | // This software is provided "as is" without warranty of any kind, | ||
30 | // either expressed or implied, including but not limited to the | ||
31 | // implied warranties of fitness for a particular purpose. | ||
32 | // | ||
33 | // See the COPYING file for more information. | ||
34 | |||
35 | #include "problem.impl.hh" | ||
36 | |||
37 | #include <coal/shape/geometric_shapes.h> | ||
38 | |||
39 | #include <boost/date_time/posix_time/posix_time_types.hpp> | ||
40 | #include <hpp/constraints/affine-function.hh> | ||
41 | #include <hpp/constraints/com-between-feet.hh> | ||
42 | #include <hpp/constraints/configuration-constraint.hh> | ||
43 | #include <hpp/constraints/convex-shape-contact.hh> | ||
44 | #include <hpp/constraints/differentiable-function.hh> | ||
45 | #include <hpp/constraints/distance-between-bodies.hh> | ||
46 | #include <hpp/constraints/generic-transformation.hh> | ||
47 | #include <hpp/constraints/implicit.hh> | ||
48 | #include <hpp/constraints/locked-joint.hh> | ||
49 | #include <hpp/constraints/manipulability.hh> | ||
50 | #include <hpp/constraints/relative-com.hh> | ||
51 | #include <hpp/constraints/symbolic-calculus.hh> | ||
52 | #include <hpp/constraints/symbolic-function.hh> | ||
53 | #include <hpp/corbaserver/server-plugin.hh> | ||
54 | #include <hpp/corbaserver/server.hh> | ||
55 | #include <hpp/core/config-projector.hh> | ||
56 | #include <hpp/core/config-validations.hh> | ||
57 | #include <hpp/core/configuration-shooter.hh> | ||
58 | #include <hpp/core/connected-component.hh> | ||
59 | #include <hpp/core/edge.hh> | ||
60 | #include <hpp/core/node.hh> | ||
61 | #include <hpp/core/parser/roadmap.hh> | ||
62 | #include <hpp/core/path-optimizer.hh> | ||
63 | #include <hpp/core/path-planner.hh> | ||
64 | #include <hpp/core/path-projector.hh> | ||
65 | #include <hpp/core/path-validation-report.hh> | ||
66 | #include <hpp/core/path-validation.hh> | ||
67 | #include <hpp/core/path-vector.hh> | ||
68 | #include <hpp/core/path.hh> | ||
69 | #include <hpp/core/plugin.hh> | ||
70 | #include <hpp/core/problem-solver.hh> | ||
71 | #include <hpp/core/problem.hh> | ||
72 | #include <hpp/core/roadmap.hh> | ||
73 | #include <hpp/core/steering-method.hh> | ||
74 | #include <hpp/core/subchain-path.hh> | ||
75 | #include <hpp/pinocchio/body.hh> | ||
76 | #include <hpp/pinocchio/center-of-mass-computation.hh> | ||
77 | #include <hpp/pinocchio/configuration.hh> | ||
78 | #include <hpp/pinocchio/serialization.hh> | ||
79 | #include <hpp/util/debug.hh> | ||
80 | #include <hpp/util/exception-factory.hh> | ||
81 | #include <hpp/util/string.hh> | ||
82 | #include <iostream> | ||
83 | #include <iterator> | ||
84 | #include <pinocchio/fwd.hpp> | ||
85 | #include <sstream> | ||
86 | |||
87 | #include "hpp/constraints_idl/constraints-fwd.hh" | ||
88 | #include "hpp/corbaserver/conversions.hh" | ||
89 | #include "hpp/corbaserver/servant-base.hh" | ||
90 | #include "hpp/core_idl/_problem-fwd.hh" | ||
91 | #include "hpp/core_idl/distances-fwd.hh" | ||
92 | #include "hpp/core_idl/path_planners-fwd.hh" | ||
93 | #include "hpp/core_idl/path_projectors-fwd.hh" | ||
94 | #include "hpp/core_idl/path_validations-fwd.hh" | ||
95 | #include "hpp/core_idl/paths-fwd.hh" | ||
96 | #include "hpp/core_idl/steering_methods-fwd.hh" | ||
97 | #include "tools.hh" | ||
98 | |||
99 | using hpp::pinocchio::CenterOfMassComputation; | ||
100 | using hpp::pinocchio::CenterOfMassComputationPtr_t; | ||
101 | using hpp::pinocchio::ObjectVector_t; | ||
102 | |||
103 | using hpp::constraints::ComBetweenFeet; | ||
104 | using hpp::constraints::ComBetweenFeetPtr_t; | ||
105 | using hpp::constraints::ConvexShapeContact; | ||
106 | using hpp::constraints::ConvexShapeContactPtr_t; | ||
107 | using hpp::constraints::DifferentiableFunctionPtr_t; | ||
108 | using hpp::constraints::DistanceBetweenBodies; | ||
109 | using hpp::constraints::LockedJoint; | ||
110 | using hpp::constraints::LockedJointPtr_t; | ||
111 | using hpp::constraints::NumericalConstraints_t; | ||
112 | using hpp::constraints::Orientation; | ||
113 | using hpp::constraints::OrientationPtr_t; | ||
114 | using hpp::constraints::Position; | ||
115 | using hpp::constraints::PositionPtr_t; | ||
116 | using hpp::constraints::RelativeCom; | ||
117 | using hpp::constraints::RelativeComPtr_t; | ||
118 | using hpp::constraints::RelativeOrientation; | ||
119 | using hpp::constraints::RelativeOrientationPtr_t; | ||
120 | using hpp::constraints::RelativePosition; | ||
121 | using hpp::constraints::RelativePositionPtr_t; | ||
122 | using hpp::constraints::RelativeTransformation; | ||
123 | using hpp::constraints::Transformation; | ||
124 | |||
125 | using hpp::constraints::Implicit; | ||
126 | using hpp::constraints::ImplicitPtr_t; | ||
127 | |||
128 | namespace hpp { | ||
129 | namespace corbaServer { | ||
130 | namespace impl { | ||
131 | namespace { | ||
132 | static const matrix3_t I3(matrix3_t ::Identity()); | ||
133 | static const vector3_t zero(vector3_t ::Zero()); | ||
134 | static const Transform3s Id(Transform3s::Identity()); | ||
135 | |||
136 | typedef hpp::core::segment_t segment_t; | ||
137 | typedef hpp::core::segments_t segments_t; | ||
138 | |||
139 | template <int PositionOrientationFlag> | ||
140 | ✗ | DifferentiableFunctionPtr_t buildGenericFunc( | |
141 | const DevicePtr_t& robot, const std::string& name, | ||
142 | const std::string& nameF1, const std::string& nameF2, const Transform3s& M1, | ||
143 | const Transform3s& M2, const std::vector<bool> mask) { | ||
144 | ✗ | if (nameF1.empty() && nameF2.empty()) | |
145 | ✗ | throw hpp::Error("At least one joint should be provided."); | |
146 | ✗ | if (nameF2.empty()) | |
147 | return buildGenericFunc<PositionOrientationFlag>(robot, name, nameF2, | ||
148 | ✗ | nameF1, M2, M1, mask); | |
149 | // From here, nameF2 is not empty. | ||
150 | |||
151 | ✗ | bool relative = (!nameF1.empty()); | |
152 | ✗ | JointPtr_t joint1, joint2; | |
153 | ✗ | Transform3s ref1 = M1, ref2 = M2; | |
154 | ✗ | if (relative) { | |
155 | ✗ | Frame frame = robot->getFrameByName(nameF1); | |
156 | ✗ | ref1 = frame.positionInParentJoint() * M1; | |
157 | ✗ | joint1 = frame.joint(); | |
158 | } | ||
159 | |||
160 | ✗ | Frame frame = robot->getFrameByName(nameF2); | |
161 | ✗ | ref2 = frame.positionInParentJoint() * M2; | |
162 | ✗ | joint2 = frame.joint(); | |
163 | |||
164 | ✗ | if (relative) { | |
165 | // Both joints are provided | ||
166 | return constraints::GenericTransformation < PositionOrientationFlag | | ||
167 | constraints::RelativeBit > | ||
168 | ✗ | ::create(name, robot, joint1, joint2, ref1, ref2, mask); | |
169 | } else { | ||
170 | return constraints::GenericTransformation<PositionOrientationFlag>::create( | ||
171 | ✗ | name, robot, joint2, ref2, ref1, mask); | |
172 | } | ||
173 | } | ||
174 | |||
175 | 3 | core::ProblemPtr_t problem(const core::ProblemSolverPtr_t& ps, | |
176 | bool throwIfNull = true) { | ||
177 | 3 | core::ProblemPtr_t p = ps->problem(); | |
178 |
3/8✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
3 | if (throwIfNull && p == NULL) throw Error("No problem in the ProblemSolver"); |
179 | 3 | return p; | |
180 | } | ||
181 | } // namespace | ||
182 | |||
183 | // --------------------------------------------------------------- | ||
184 | |||
185 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
16 | Problem::Problem() : server_(NULL) {} |
186 | |||
187 | // --------------------------------------------------------------- | ||
188 | |||
189 | ✗ | void Problem::setMaxNumThreads(UShort n) { | |
190 | try { | ||
191 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
192 | ✗ | robot->numberDeviceData((size_type)n); | |
193 | ✗ | } catch (const std::exception& e) { | |
194 | ✗ | throw Error(e.what()); | |
195 | } | ||
196 | } | ||
197 | |||
198 | // --------------------------------------------------------------- | ||
199 | |||
200 | ✗ | UShort Problem::getMaxNumThreads() { | |
201 | try { | ||
202 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
203 | ✗ | return (UShort)robot->numberDeviceData(); | |
204 | ✗ | } catch (const std::exception& e) { | |
205 | ✗ | throw Error(e.what()); | |
206 | } | ||
207 | } | ||
208 | |||
209 | // --------------------------------------------------------------- | ||
210 | |||
211 | 8 | core::ProblemSolverPtr_t Problem::problemSolver() { | |
212 | 8 | return server_->problemSolver(); | |
213 | } | ||
214 | |||
215 | // --------------------------------------------------------------- | ||
216 | |||
217 | #define _NOOP ((void)0) | ||
218 | #define _CASE(test, op_true, op_false) \ | ||
219 | if (!ok) { \ | ||
220 | if (util::iequal(w, test)) { \ | ||
221 | op_true; \ | ||
222 | ok = true; \ | ||
223 | } else { \ | ||
224 | op_false; \ | ||
225 | } \ | ||
226 | } | ||
227 | #define _CASE_SET_RET(name, val_true) \ | ||
228 | _CASE(name, ret = val_true, types.push_back(name)) | ||
229 | #define _CASE_PS_MAP(name, map) \ | ||
230 | _CASE_SET_RET(name, problemSolver()->map.getKeys<Ret_t>()) | ||
231 | |||
232 | ✗ | Names_t* Problem::getAvailable(const char* what) { | |
233 | ✗ | std::string w(what); | |
234 | typedef std::list<std::string> Ret_t; | ||
235 | ✗ | Ret_t ret, types; | |
236 | ✗ | bool ok = false; | |
237 | |||
238 | ✗ | _CASE_PS_MAP("PathOptimizer", pathOptimizers) | |
239 | ✗ | _CASE_PS_MAP("PathProjector", pathProjectors) | |
240 | ✗ | _CASE_PS_MAP("PathPlanner", pathPlanners) | |
241 | ✗ | _CASE_PS_MAP("ConfigurationShooter", configurationShooters) | |
242 | ✗ | _CASE_PS_MAP("PathValidation", pathValidations) | |
243 | ✗ | _CASE_PS_MAP("ConfigValidation", configValidations) | |
244 | ✗ | _CASE_PS_MAP("SteeringMethod", steeringMethods) | |
245 | ✗ | _CASE_PS_MAP("Distance", distances) | |
246 | ✗ | _CASE_PS_MAP("NumericalConstraint", numericalConstraints) | |
247 | ✗ | _CASE_PS_MAP("CenterOfMass", centerOfMassComputations) | |
248 | ✗ | _CASE_SET_RET("Problem", server_->problemSolverMap()->keys<Ret_t>()) | |
249 | ✗ | _CASE_SET_RET("Parameter", | |
250 | problem(problemSolver())->parameters.getKeys<Ret_t>()) | ||
251 | ✗ | _CASE_SET_RET( | |
252 | "DefaultParameter", | ||
253 | problem(problemSolver())->parameterDescriptions().getKeys<Ret_t>()) | ||
254 | ✗ | _CASE_SET_RET("Type", types) | |
255 | |||
256 | ✗ | if (!ok) { | |
257 | ✗ | throw Error(("Type \"" + std::string(what) + "\" not known").c_str()); | |
258 | } | ||
259 | |||
260 | ✗ | return toNames_t(ret.begin(), ret.end()); | |
261 | } | ||
262 | |||
263 | // --------------------------------------------------------------- | ||
264 | |||
265 | #define _CASE_PUSH_TO(name, val) \ | ||
266 | _CASE(name, ret.push_back(val), types.push_back(name)) | ||
267 | |||
268 | ✗ | Names_t* Problem::getSelected(const char* what) { | |
269 | ✗ | std::string w(what); | |
270 | typedef std::vector<std::string> Ret_t; | ||
271 | ✗ | Ret_t ret, types; | |
272 | ✗ | bool ok = false; | |
273 | core::value_type tol; | ||
274 | |||
275 | ✗ | _CASE_SET_RET("PathOptimizer", | |
276 | Ret_t(problemSolver()->pathOptimizerTypes().begin(), | ||
277 | problemSolver()->pathOptimizerTypes().end())) | ||
278 | ✗ | _CASE_SET_RET("ConfigValidation", | |
279 | Ret_t(problemSolver()->configValidationTypes())) | ||
280 | ✗ | _CASE_PUSH_TO("PathProjector", problemSolver()->pathProjectorType(tol)) | |
281 | ✗ | _CASE_PUSH_TO("PathPlanner", problemSolver()->pathPlannerType()) | |
282 | ✗ | _CASE_PUSH_TO("ConfigurationShooter", | |
283 | problemSolver()->configurationShooterType()) | ||
284 | ✗ | _CASE_PUSH_TO("PathValidation", problemSolver()->pathValidationType(tol)) | |
285 | ✗ | _CASE_PUSH_TO("SteeringMethod", problemSolver()->steeringMethodType()) | |
286 | ✗ | _CASE_PUSH_TO("Distance", problemSolver()->distanceType()) | |
287 | ✗ | _CASE_PUSH_TO("Problem", server_->problemSolverMap()->selectedName()) | |
288 | ✗ | _CASE_SET_RET("Type", types) | |
289 | ✗ | if (!ok) { | |
290 | ✗ | throw Error(("Type \"" + std::string(what) + "\" not known").c_str()); | |
291 | } | ||
292 | |||
293 | ✗ | return toNames_t(ret.begin(), ret.end()); | |
294 | } | ||
295 | |||
296 | #undef _CASE | ||
297 | #undef _CASE_SET_RET | ||
298 | #undef _CASE_PUSH_TO | ||
299 | #undef _CASE_PS_MAP | ||
300 | #undef _NOOP | ||
301 | |||
302 | // --------------------------------------------------------------- | ||
303 | |||
304 | ✗ | void Problem::setParameter(const char* name, const CORBA::Any& value) { | |
305 | ✗ | if (problemSolver()->problem() != NULL) { | |
306 | try { | ||
307 | ✗ | problemSolver()->problem()->setParameter(std::string(name), | |
308 | ✗ | toParameter(value)); | |
309 | ✗ | } catch (const std::exception& e) { | |
310 | ✗ | throw hpp::Error(e.what()); | |
311 | } | ||
312 | ✗ | return; | |
313 | } | ||
314 | ✗ | throw Error("No problem in the ProblemSolver"); | |
315 | } | ||
316 | |||
317 | // --------------------------------------------------------------- | ||
318 | |||
319 | ✗ | CORBA::Any* Problem::getParameter(const char* name) { | |
320 | try { | ||
321 | ✗ | core::ProblemPtr_t p(problemSolver()->problem()); | |
322 | ✗ | if (p) return toCorbaAnyPtr(p->getParameter(name)); | |
323 | ✗ | throw hpp::Error("The problem is not initialized."); | |
324 | ✗ | } catch (std::exception& e) { | |
325 | ✗ | throw Error(e.what()); | |
326 | } | ||
327 | } | ||
328 | |||
329 | // --------------------------------------------------------------- | ||
330 | |||
331 | ✗ | char* Problem::getParameterDoc(const char* name) { | |
332 | ✗ | if (problemSolver()->problem() != NULL) { | |
333 | try { | ||
334 | const core::ParameterDescription& desc = | ||
335 | ✗ | core::Problem::parameterDescription(name); | |
336 | ✗ | std::stringstream ss; | |
337 | ✗ | ss << desc.doc() << " [Default: "; | |
338 | try { | ||
339 | ✗ | ss << desc.defaultValue(); | |
340 | ✗ | } catch (const std::logic_error&) { | |
341 | ✗ | ss << "None"; | |
342 | } | ||
343 | ✗ | ss << ']'; | |
344 | ✗ | return c_str(ss.str()); | |
345 | ✗ | } catch (const std::exception& e) { | |
346 | ✗ | throw hpp::Error(e.what()); | |
347 | } | ||
348 | } | ||
349 | ✗ | throw Error("No problem in the ProblemSolver"); | |
350 | } | ||
351 | |||
352 | // --------------------------------------------------------------- | ||
353 | |||
354 | ✗ | bool Problem::selectProblem(const char* name) { | |
355 | ✗ | std::string psName(name); | |
356 | ✗ | ProblemSolverMapPtr_t psMap(server_->problemSolverMap()); | |
357 | ✗ | bool has = psMap->has(psName); | |
358 | ✗ | if (!has) psMap->add(psName, core::ProblemSolver::create()); | |
359 | ✗ | psMap->selected(psName); | |
360 | ✗ | return !has; | |
361 | } | ||
362 | |||
363 | // --------------------------------------------------------------- | ||
364 | |||
365 | ✗ | void Problem::resetProblem() { | |
366 | ✗ | ProblemSolverMapPtr_t psMap(server_->problemSolverMap()); | |
367 | ✗ | psMap->replaceSelected(core::ProblemSolver::create()); | |
368 | } | ||
369 | |||
370 | // --------------------------------------------------------------- | ||
371 | |||
372 | ✗ | bool Problem::loadPlugin(const char* pluginName) { | |
373 | try { | ||
374 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
375 | ✗ | std::string libname = core::plugin::findPluginLibrary(pluginName); | |
376 | ✗ | return core::plugin::loadPlugin(libname, ps); | |
377 | ✗ | } catch (std::exception& exc) { | |
378 | ✗ | throw Error(exc.what()); | |
379 | } | ||
380 | } | ||
381 | |||
382 | // --------------------------------------------------------------- | ||
383 | |||
384 | ✗ | void Problem::movePathToProblem(ULong pathId, const char* name, | |
385 | const Names_t& jointNames) { | ||
386 | ✗ | ProblemSolverMapPtr_t psMap(server_->problemSolverMap()); | |
387 | ✗ | if (!psMap->has(std::string(name))) | |
388 | ✗ | throw Error("No ProblemSolver of this name"); | |
389 | ✗ | core::ProblemSolverPtr_t current = problemSolver(), | |
390 | ✗ | other = psMap->get(std::string(name)); | |
391 | |||
392 | ✗ | if (pathId >= current->paths().size()) { | |
393 | ✗ | std::ostringstream oss("wrong path id: "); | |
394 | ✗ | oss << pathId << ", number path: " << current->paths().size() << "."; | |
395 | ✗ | throw Error(oss.str().c_str()); | |
396 | } | ||
397 | |||
398 | ✗ | core::PathVectorPtr_t pv; | |
399 | ✗ | if (jointNames.length() == 0) { | |
400 | ✗ | pv = current->paths()[pathId]; | |
401 | } else { | ||
402 | ✗ | segments_t cInts, vInts; | |
403 | ✗ | for (CORBA::ULong i = 0; i < jointNames.length(); ++i) { | |
404 | const Frame frame = | ||
405 | ✗ | problemSolver()->robot()->getFrameByName(std::string(jointNames[i])); | |
406 | ✗ | if (!frame.isFixed()) { | |
407 | ✗ | JointPtr_t joint = frame.joint(); | |
408 | ✗ | if (joint == NULL) { | |
409 | ✗ | std::ostringstream oss; | |
410 | ✗ | oss << "Joint " << jointNames[i] << "not found."; | |
411 | ✗ | throw hpp::Error(oss.str().c_str()); | |
412 | } | ||
413 | ✗ | cInts.push_back( | |
414 | ✗ | segment_t(joint->rankInConfiguration(), joint->configSize())); | |
415 | ✗ | vInts.push_back(segment_t(joint->rankInVelocity(), joint->numberDof())); | |
416 | } | ||
417 | } | ||
418 | |||
419 | ✗ | Eigen::BlockIndex::sort(cInts); | |
420 | ✗ | Eigen::BlockIndex::sort(vInts); | |
421 | ✗ | Eigen::BlockIndex::shrink(cInts); | |
422 | ✗ | Eigen::BlockIndex::shrink(vInts); | |
423 | |||
424 | core::SubchainPathPtr_t nPath = | ||
425 | ✗ | core::SubchainPath::create(current->paths()[pathId], cInts, vInts); | |
426 | ✗ | pv = core::PathVector::create(nPath->outputSize(), | |
427 | ✗ | nPath->outputDerivativeSize()); | |
428 | ✗ | pv->appendPath(nPath); | |
429 | } | ||
430 | ✗ | other->addPath(pv); | |
431 | } | ||
432 | |||
433 | // --------------------------------------------------------------- | ||
434 | |||
435 | ✗ | void Problem::setInitialConfig(const hpp::floatSeq& dofArray) { | |
436 | try { | ||
437 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
438 | ✗ | Configuration_t config = floatSeqToConfig(robot, dofArray, true); | |
439 | ✗ | problemSolver()->initConfig(config); | |
440 | ✗ | } catch (const std::exception& exc) { | |
441 | ✗ | throw hpp::Error(exc.what()); | |
442 | } | ||
443 | } | ||
444 | |||
445 | // --------------------------------------------------------------- | ||
446 | |||
447 | ✗ | hpp::floatSeq* Problem::getInitialConfig() { | |
448 | hpp::floatSeq* dofArray; | ||
449 | |||
450 | // Get robot in hppPlanner object. | ||
451 | ✗ | Configuration_t config = problemSolver()->initConfig(); | |
452 | |||
453 | ✗ | std::size_t deviceDim = config.size(); | |
454 | ✗ | if (deviceDim > 0) { | |
455 | ✗ | dofArray = new hpp::floatSeq(); | |
456 | ✗ | dofArray->length((CORBA::ULong)deviceDim); | |
457 | |||
458 | ✗ | for (unsigned int i = 0; i < deviceDim; i++) { | |
459 | ✗ | (*dofArray)[i] = (config)[i]; | |
460 | } | ||
461 | ✗ | return dofArray; | |
462 | } else { | ||
463 | ✗ | throw hpp::Error("no initial configuration defined"); | |
464 | } | ||
465 | } | ||
466 | |||
467 | // --------------------------------------------------------------- | ||
468 | |||
469 | ✗ | void Problem::addGoalConfig(const hpp::floatSeq& dofArray) { | |
470 | try { | ||
471 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
472 | ✗ | Configuration_t config = floatSeqToConfig(robot, dofArray, true); | |
473 | ✗ | problemSolver()->addGoalConfig(config); | |
474 | ✗ | } catch (const std::exception& exc) { | |
475 | ✗ | throw hpp::Error(exc.what()); | |
476 | } | ||
477 | } | ||
478 | |||
479 | // --------------------------------------------------------------- | ||
480 | |||
481 | ✗ | hpp::floatSeqSeq* Problem::getGoalConfigs() { | |
482 | try { | ||
483 | hpp::floatSeqSeq* configSequence; | ||
484 | ✗ | const core::Configurations_t goalConfigs(problemSolver()->goalConfigs()); | |
485 | ✗ | std::size_t nbGoalConfig = goalConfigs.size(); | |
486 | ✗ | configSequence = new hpp::floatSeqSeq(); | |
487 | ✗ | configSequence->length((CORBA::ULong)nbGoalConfig); | |
488 | ✗ | for (std::size_t i = 0; i < nbGoalConfig; ++i) { | |
489 | ✗ | const Configuration_t& config = goalConfigs[i]; | |
490 | ✗ | std::size_t deviceDim = config.size(); | |
491 | |||
492 | ✗ | hpp::floatSeq dofArray; | |
493 | ✗ | dofArray.length((CORBA::ULong)deviceDim); | |
494 | |||
495 | ✗ | for (std::size_t j = 0; j < deviceDim; ++j) | |
496 | ✗ | dofArray[(CORBA::ULong)j] = (config)[j]; | |
497 | ✗ | (*configSequence)[(CORBA::ULong)i] = dofArray; | |
498 | } | ||
499 | ✗ | return configSequence; | |
500 | ✗ | } catch (const std::exception& exc) { | |
501 | ✗ | throw hpp::Error(exc.what()); | |
502 | } | ||
503 | } | ||
504 | |||
505 | // --------------------------------------------------------------- | ||
506 | |||
507 | ✗ | void Problem::resetGoalConfigs() { problemSolver()->resetGoalConfigs(); } | |
508 | |||
509 | ✗ | void Problem::setGoalConstraints(const Names_t& constraints) { | |
510 | try { | ||
511 | ✗ | NumericalConstraints_t ncs; | |
512 | ✗ | for (CORBA::ULong i = 0; i < constraints.length(); ++i) { | |
513 | ImplicitPtr_t c( | ||
514 | ✗ | problemSolver()->numericalConstraint(std::string(constraints[i]))); | |
515 | ✗ | if (!c) { | |
516 | ✗ | std::ostringstream os; | |
517 | ✗ | os << "Constraint \"" << constraints[i] << "\" not found."; | |
518 | ✗ | throw std::invalid_argument(os.str().c_str()); | |
519 | } | ||
520 | ✗ | ncs.push_back(c); | |
521 | } | ||
522 | ✗ | problemSolver()->setGoalConstraints(ncs); | |
523 | ✗ | } catch (const std::exception& exc) { | |
524 | ✗ | throw hpp::Error(exc.what()); | |
525 | } | ||
526 | } | ||
527 | |||
528 | ✗ | void Problem::resetGoalConstraints() { | |
529 | try { | ||
530 | ✗ | problemSolver()->resetGoalConstraints(); | |
531 | ✗ | } catch (const std::exception& exc) { | |
532 | ✗ | throw hpp::Error(exc.what()); | |
533 | } | ||
534 | } | ||
535 | |||
536 | // --------------------------------------------------------------- | ||
537 | ✗ | static int numberOfTrue(const hpp::boolSeq& mask) { | |
538 | ✗ | std::size_t res = 0; | |
539 | ✗ | for (std::size_t i = 0; i < mask.length(); ++i) | |
540 | ✗ | if (mask[(CORBA::ULong)i]) ++res; | |
541 | ✗ | return (int)res; | |
542 | } | ||
543 | // --------------------------------------------------------------- | ||
544 | |||
545 | ✗ | void Problem::createOrientationConstraint(const char* constraintName, | |
546 | const char* joint1Name, | ||
547 | const char* joint2Name, | ||
548 | const Double* p, | ||
549 | const hpp::boolSeq& mask) { | ||
550 | try { | ||
551 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
552 | ✗ | Transform3s::Quaternion quat(p[3], p[0], p[1], p[2]); | |
553 | ✗ | Transform3s rotation(quat.matrix(), vector3_t::Zero()); | |
554 | ✗ | std::string name(constraintName); | |
555 | |||
556 | DifferentiableFunctionPtr_t func = | ||
557 | buildGenericFunc<constraints::OrientationBit>( | ||
558 | ✗ | problemSolver()->robot(), name, joint1Name, joint2Name, Id, | |
559 | ✗ | rotation, boolSeqToVector(mask)); | |
560 | |||
561 | ✗ | problemSolver()->addNumericalConstraint( | |
562 | name, | ||
563 | ✗ | Implicit::create(func, numberOfTrue(mask) * constraints::EqualToZero)); | |
564 | ✗ | } catch (const std::exception& exc) { | |
565 | ✗ | throw hpp::Error(exc.what()); | |
566 | } | ||
567 | } | ||
568 | |||
569 | // --------------------------------------------------------------- | ||
570 | |||
571 | ✗ | void Problem::createTransformationConstraint(const char* constraintName, | |
572 | const char* joint1Name, | ||
573 | const char* joint2Name, | ||
574 | const Transform_ p, | ||
575 | const hpp::boolSeq& mask) { | ||
576 | try { | ||
577 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
578 | ✗ | Transform3s ref(toTransform3s(p)); | |
579 | ✗ | std::string name(constraintName); | |
580 | DifferentiableFunctionPtr_t func = | ||
581 | buildGenericFunc < constraints::PositionBit | | ||
582 | ✗ | constraints::OrientationBit > (problemSolver()->robot(), name, | |
583 | joint1Name, joint2Name, ref, Id, | ||
584 | ✗ | boolSeqToVector(mask, 6)); | |
585 | |||
586 | ✗ | problemSolver()->addNumericalConstraint( | |
587 | name, | ||
588 | ✗ | Implicit::create(func, numberOfTrue(mask) * constraints::EqualToZero)); | |
589 | ✗ | } catch (const std::exception& exc) { | |
590 | ✗ | throw hpp::Error(exc.what()); | |
591 | } | ||
592 | } | ||
593 | |||
594 | // --------------------------------------------------------------- | ||
595 | |||
596 | ✗ | void Problem::createTransformationConstraint2(const char* constraintName, | |
597 | const char* joint1Name, | ||
598 | const char* joint2Name, | ||
599 | const Transform_ frame1, | ||
600 | const Transform_ frame2, | ||
601 | const hpp::boolSeq& mask) { | ||
602 | try { | ||
603 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
604 | ✗ | std::string name(constraintName); | |
605 | DifferentiableFunctionPtr_t func = | ||
606 | buildGenericFunc < constraints::PositionBit | | ||
607 | constraints::OrientationBit > | ||
608 | ✗ | (problemSolver()->robot(), name, joint1Name, joint2Name, | |
609 | ✗ | toTransform3s(frame1), toTransform3s(frame2), | |
610 | ✗ | boolSeqToVector(mask, 6)); | |
611 | |||
612 | ✗ | problemSolver()->addNumericalConstraint( | |
613 | name, | ||
614 | ✗ | Implicit::create(func, numberOfTrue(mask) * constraints::EqualToZero)); | |
615 | ✗ | } catch (const std::exception& exc) { | |
616 | ✗ | throw hpp::Error(exc.what()); | |
617 | } | ||
618 | } | ||
619 | |||
620 | // --------------------------------------------------------------- | ||
621 | |||
622 | ✗ | void Problem::createTransformationR3xSO3Constraint(const char* constraintName, | |
623 | const char* joint1Name, | ||
624 | const char* joint2Name, | ||
625 | const Transform_ frame1, | ||
626 | const Transform_ frame2, | ||
627 | const hpp::boolSeq& mask) { | ||
628 | try { | ||
629 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
630 | ✗ | std::string name(constraintName); | |
631 | DifferentiableFunctionPtr_t func = | ||
632 | buildGenericFunc < constraints::OutputR3xSO3Bit | | ||
633 | constraints::PositionBit | | ||
634 | constraints::OrientationBit > | ||
635 | ✗ | (problemSolver()->robot(), name, joint1Name, joint2Name, | |
636 | ✗ | toTransform3s(frame1), toTransform3s(frame2), | |
637 | ✗ | boolSeqToVector(mask, 6)); | |
638 | |||
639 | ✗ | problemSolver()->addNumericalConstraint( | |
640 | name, | ||
641 | ✗ | Implicit::create(func, numberOfTrue(mask) * constraints::EqualToZero)); | |
642 | ✗ | } catch (const std::exception& exc) { | |
643 | ✗ | throw hpp::Error(exc.what()); | |
644 | } | ||
645 | } | ||
646 | |||
647 | // --------------------------------------------------------------- | ||
648 | |||
649 | ✗ | void Problem::createLockedJoint(const char* lockedJointName, | |
650 | const char* jointName, | ||
651 | const hpp::floatSeq& value) { | ||
652 | try { | ||
653 | // Get robot in hppPlanner object. | ||
654 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
655 | ✗ | JointPtr_t joint = robot->getJointByName(jointName); | |
656 | ✗ | vector_t config = floatSeqToVector(value, joint->configSize()); | |
657 | hppDout(info, "joint->configurationSpace ()->name () = " | ||
658 | << joint->configurationSpace()->name()); | ||
659 | hppDout(info, "joint->configurationSpace ()->nq () = " | ||
660 | << joint->configurationSpace()->nq()); | ||
661 | ✗ | LiegroupElement lge(config, joint->configurationSpace()); | |
662 | ✗ | LockedJointPtr_t lockedJoint(LockedJoint::create(joint, lge)); | |
663 | ✗ | problemSolver()->numericalConstraints.add(lockedJointName, lockedJoint); | |
664 | ✗ | } catch (const std::exception& exc) { | |
665 | ✗ | throw hpp::Error(exc.what()); | |
666 | } | ||
667 | } | ||
668 | |||
669 | // --------------------------------------------------------------- | ||
670 | |||
671 | ✗ | void Problem::createLockedJointWithComp(const char* lockedJointName, | |
672 | const char* jointName, | ||
673 | const hpp::floatSeq& value, | ||
674 | const hpp::ComparisonTypes_t& comp) { | ||
675 | try { | ||
676 | // Get robot in hppPlanner object. | ||
677 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
678 | ✗ | JointPtr_t joint = robot->getJointByName(jointName); | |
679 | ✗ | vector_t config = floatSeqToVector(value, joint->configSize()); | |
680 | hppDout(info, "joint->configurationSpace ()->name () = " | ||
681 | << joint->configurationSpace()->name()); | ||
682 | hppDout(info, "joint->configurationSpace ()->nq () = " | ||
683 | << joint->configurationSpace()->nq()); | ||
684 | ✗ | LiegroupElement lge(config, joint->configurationSpace()); | |
685 | ✗ | LockedJointPtr_t lockedJoint(LockedJoint::create(joint, lge)); | |
686 | ✗ | lockedJoint->comparisonType(convertComparison(comp)); | |
687 | ✗ | problemSolver()->numericalConstraints.add(lockedJointName, lockedJoint); | |
688 | ✗ | } catch (const std::exception& exc) { | |
689 | ✗ | throw hpp::Error(exc.what()); | |
690 | } | ||
691 | } | ||
692 | |||
693 | // --------------------------------------------------------------- | ||
694 | |||
695 | ✗ | void Problem::createLockedExtraDof(const char* lockedDofName, | |
696 | const CORBA::ULong index, | ||
697 | const hpp::floatSeq& value) { | ||
698 | try { | ||
699 | // Get robot in hppPlanner object. | ||
700 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
701 | ✗ | vector_t config = floatSeqToVector(value); | |
702 | |||
703 | ✗ | LockedJointPtr_t lockedJoint(LockedJoint::create(robot, index, config)); | |
704 | ✗ | problemSolver()->numericalConstraints.add(lockedDofName, lockedJoint); | |
705 | ✗ | } catch (const std::exception& exc) { | |
706 | ✗ | throw hpp::Error(exc.what()); | |
707 | } | ||
708 | } | ||
709 | |||
710 | // --------------------------------------------------------------- | ||
711 | |||
712 | ✗ | void Problem::createManipulability(const char* _name, const char* _function) { | |
713 | try { | ||
714 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
715 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
716 | |||
717 | ✗ | std::string function(_function), name(_name); | |
718 | ✗ | if (!ps->numericalConstraints.has(function)) | |
719 | ✗ | throw Error(("Constraint " + function + " not found").c_str()); | |
720 | DifferentiableFunctionPtr_t f( | ||
721 | ✗ | ps->numericalConstraints.get(function)->functionPtr()); | |
722 | |||
723 | ✗ | ps->addNumericalConstraint( | |
724 | name, | ||
725 | ✗ | Implicit::create(constraints::Manipulability::create(f, robot, name), | |
726 | ✗ | 1 * constraints::EqualToZero)); | |
727 | ✗ | } catch (const std::exception& exc) { | |
728 | ✗ | throw hpp::Error(exc.what()); | |
729 | } | ||
730 | } | ||
731 | |||
732 | // --------------------------------------------------------------- | ||
733 | |||
734 | ✗ | void Problem::createRelativeComConstraint(const char* constraintName, | |
735 | const char* comName, | ||
736 | const char* jointName, | ||
737 | const floatSeq& p, | ||
738 | const hpp::boolSeq& mask) { | ||
739 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
740 | ✗ | JointPtr_t joint; | |
741 | ✗ | CenterOfMassComputationPtr_t comc; | |
742 | ✗ | vector3_t point = floatSeqToVector3(p); | |
743 | |||
744 | ✗ | std::vector<bool> m = boolSeqToVector(mask, 3); | |
745 | try { | ||
746 | ✗ | joint = problemSolver()->robot()->getJointByName(jointName); | |
747 | // Test whether joint1 is world frame | ||
748 | ✗ | std::string name(constraintName), comN(comName); | |
749 | ✗ | if (comN.compare("") == 0) { | |
750 | ✗ | problemSolver()->addNumericalConstraint( | |
751 | name, | ||
752 | ✗ | Implicit::create(RelativeCom::create(name, problemSolver()->robot(), | |
753 | joint, point, m), | ||
754 | ✗ | numberOfTrue(mask) * constraints::EqualToZero)); | |
755 | } else { | ||
756 | ✗ | if (!problemSolver()->centerOfMassComputations.has(comN)) | |
757 | ✗ | throw hpp::Error("Partial COM not found."); | |
758 | ✗ | comc = problemSolver()->centerOfMassComputations.get(comN); | |
759 | ✗ | problemSolver()->addNumericalConstraint( | |
760 | name, | ||
761 | ✗ | Implicit::create(RelativeCom::create(name, problemSolver()->robot(), | |
762 | comc, joint, point, m), | ||
763 | ✗ | numberOfTrue(mask) * constraints::EqualToZero)); | |
764 | } | ||
765 | ✗ | } catch (const std::exception& exc) { | |
766 | ✗ | throw hpp::Error(exc.what()); | |
767 | } | ||
768 | } | ||
769 | |||
770 | // --------------------------------------------------------------- | ||
771 | |||
772 | ✗ | void Problem::createComBeetweenFeet( | |
773 | const char* constraintName, const char* comName, const char* jointLName, | ||
774 | const char* jointRName, const floatSeq& pL, const floatSeq& pR, | ||
775 | const char* jointRefName, const floatSeq& pRef, const hpp::boolSeq& mask) { | ||
776 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
777 | ✗ | JointPtr_t jointL, jointR, jointRef; | |
778 | ✗ | CenterOfMassComputationPtr_t comc; | |
779 | ✗ | vector3_t pointL = floatSeqToVector3(pL); | |
780 | ✗ | vector3_t pointR = floatSeqToVector3(pR); | |
781 | ✗ | vector3_t pointRef = floatSeqToVector3(pRef); | |
782 | |||
783 | ✗ | std::vector<bool> m = boolSeqToVector(mask); | |
784 | try { | ||
785 | ✗ | jointL = problemSolver()->robot()->getJointByName(jointLName); | |
786 | ✗ | jointR = problemSolver()->robot()->getJointByName(jointRName); | |
787 | // Test whether joint1 is world frame | ||
788 | ✗ | if (std::string(jointRefName) == std::string("")) | |
789 | ✗ | jointRef = problemSolver()->robot()->rootJoint(); | |
790 | else | ||
791 | ✗ | jointRef = problemSolver()->robot()->getJointByName(jointRefName); | |
792 | ✗ | std::string name(constraintName), comN(comName); | |
793 | |||
794 | ✗ | constraints::ComparisonTypes_t comps(2 * constraints::EqualToZero | |
795 | ✗ | << constraints::Superior | |
796 | ✗ | << constraints::Inferior); | |
797 | |||
798 | ✗ | if (comN.compare("") == 0) { | |
799 | ✗ | problemSolver()->addNumericalConstraint( | |
800 | name, | ||
801 | ✗ | Implicit::create(ComBetweenFeet::create( | |
802 | ✗ | name, problemSolver()->robot(), jointL, jointR, | |
803 | pointL, pointR, jointRef, pointRef, m), | ||
804 | comps)); | ||
805 | } else { | ||
806 | ✗ | if (!problemSolver()->centerOfMassComputations.has(comN)) | |
807 | ✗ | throw hpp::Error("Partial COM not found."); | |
808 | ✗ | comc = problemSolver()->centerOfMassComputations.get(comN); | |
809 | ✗ | problemSolver()->addNumericalConstraint( | |
810 | name, | ||
811 | ✗ | Implicit::create(ComBetweenFeet::create( | |
812 | ✗ | name, problemSolver()->robot(), comc, jointL, | |
813 | jointR, pointL, pointR, jointRef, pointRef, m), | ||
814 | comps)); | ||
815 | } | ||
816 | ✗ | } catch (const std::exception& exc) { | |
817 | ✗ | throw hpp::Error(exc.what()); | |
818 | } | ||
819 | } | ||
820 | |||
821 | // --------------------------------------------------------------- | ||
822 | |||
823 | ✗ | void Problem::createConvexShapeContactConstraint( | |
824 | const char* constraintName, const Names_t& floorJoints, | ||
825 | const Names_t& objectJoints, const hpp::floatSeqSeq& points, | ||
826 | const hpp::intSeqSeq& objTriangles, const hpp::intSeqSeq& floorTriangles) { | ||
827 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
828 | try { | ||
829 | ✗ | JointAndShapes_t floorSurfaces, objectSurfaces; | |
830 | ✗ | std::vector<coal::Vec3f> pts(points.length()); | |
831 | ✗ | for (CORBA::ULong i = 0; i < points.length(); ++i) { | |
832 | ✗ | if (points[i].length() != 3) | |
833 | ✗ | throw hpp::Error("Points must be of size 3."); | |
834 | ✗ | pts[i] = coal::Vec3f(points[i][0], points[i][1], points[i][2]); | |
835 | } | ||
836 | ✗ | if (objectJoints.length() != objTriangles.length()) { | |
837 | ✗ | std::ostringstream oss; | |
838 | ✗ | oss << "Number of object joints (" << objectJoints.length() | |
839 | ✗ | << ") should fit number of objTriangles (" << objTriangles.length() | |
840 | ✗ | << ")."; | |
841 | ✗ | throw std::runtime_error(oss.str()); | |
842 | } | ||
843 | ✗ | for (CORBA::ULong i = 0; i < objTriangles.length(); ++i) { | |
844 | ✗ | if (objTriangles[i].length() != 3) | |
845 | ✗ | throw hpp::Error("Triangle must have size 3."); | |
846 | |||
847 | ✗ | for (size_t j = 0; j < 3; j++) | |
848 | ✗ | if (objTriangles[i][(CORBA::ULong)j] < 0 && | |
849 | ✗ | (size_t)objTriangles[i][(CORBA::ULong)j] >= pts.size()) | |
850 | ✗ | throw hpp::Error("Point index out of range."); | |
851 | |||
852 | ✗ | std::string jointName(objectJoints[i]); | |
853 | ✗ | JointPtr_t joint; | |
854 | ✗ | if (jointName == "None") { | |
855 | // joint = 0x0; | ||
856 | } else { | ||
857 | ✗ | joint = problemSolver()->robot()->getJointByName(jointName); | |
858 | } | ||
859 | ✗ | std::vector<core::vector3_t> shapePts{pts[objTriangles[i][0]], | |
860 | ✗ | pts[objTriangles[i][1]], | |
861 | ✗ | pts[objTriangles[i][2]]}; | |
862 | ✗ | objectSurfaces.push_back(JointAndShape_t(joint, shapePts)); | |
863 | } | ||
864 | ✗ | if (floorJoints.length() != floorTriangles.length()) { | |
865 | ✗ | std::ostringstream oss; | |
866 | ✗ | oss << "Number of floor joints (" << floorJoints.length() | |
867 | ✗ | << ") should fit number of floorTriangles (" | |
868 | ✗ | << floorTriangles.length() << ")."; | |
869 | ✗ | throw std::runtime_error(oss.str()); | |
870 | } | ||
871 | ✗ | for (CORBA::ULong i = 0; i < floorTriangles.length(); ++i) { | |
872 | ✗ | if (floorTriangles[i].length() != 3) | |
873 | ✗ | throw hpp::Error("Triangle must have size 3."); | |
874 | ✗ | for (size_t j = 0; j < 3; j++) | |
875 | ✗ | if (floorTriangles[i][(CORBA::ULong)j] < 0 && | |
876 | ✗ | (size_t)floorTriangles[i][(CORBA::ULong)j] >= pts.size()) | |
877 | ✗ | throw hpp::Error("Point index out of range."); | |
878 | |||
879 | ✗ | std::string jointName(floorJoints[i]); | |
880 | ✗ | JointPtr_t joint; | |
881 | ✗ | if (jointName == "None") { | |
882 | // joint = 0x0; | ||
883 | } else { | ||
884 | ✗ | joint = problemSolver()->robot()->getJointByName(jointName); | |
885 | } | ||
886 | ✗ | std::vector<core::vector3_t> shapePts{pts[floorTriangles[i][0]], | |
887 | ✗ | pts[floorTriangles[i][1]], | |
888 | ✗ | pts[floorTriangles[i][2]]}; | |
889 | ✗ | floorSurfaces.push_back(JointAndShape_t(joint, shapePts)); | |
890 | } | ||
891 | ✗ | std::string name(constraintName); | |
892 | ConvexShapeContactPtr_t f = ConvexShapeContact::create( | ||
893 | ✗ | name, problemSolver()->robot(), floorSurfaces, objectSurfaces); | |
894 | ✗ | problemSolver()->addNumericalConstraint( | |
895 | ✗ | name, Implicit::create(f, 5 * constraints::EqualToZero)); | |
896 | ✗ | } catch (const std::exception& exc) { | |
897 | ✗ | throw hpp::Error(exc.what()); | |
898 | } | ||
899 | } | ||
900 | |||
901 | // --------------------------------------------------------------- | ||
902 | |||
903 | ✗ | void Problem::createPositionConstraint(const char* constraintName, | |
904 | const char* joint1Name, | ||
905 | const char* joint2Name, | ||
906 | const hpp::floatSeq& point1, | ||
907 | const hpp::floatSeq& point2, | ||
908 | const hpp::boolSeq& mask) { | ||
909 | try { | ||
910 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
911 | ✗ | vector3_t p1 = floatSeqToVector3(point1); | |
912 | ✗ | vector3_t p2 = floatSeqToVector3(point2); | |
913 | ✗ | std::string name(constraintName); | |
914 | |||
915 | DifferentiableFunctionPtr_t func = | ||
916 | buildGenericFunc<constraints::PositionBit>( | ||
917 | ✗ | problemSolver()->robot(), name, joint1Name, joint2Name, | |
918 | ✗ | Transform3s(I3, p1), Transform3s(I3, p2), boolSeqToVector(mask)); | |
919 | |||
920 | ✗ | problemSolver()->addNumericalConstraint( | |
921 | name, | ||
922 | ✗ | Implicit::create(func, numberOfTrue(mask) * constraints::EqualToZero)); | |
923 | ✗ | } catch (const std::exception& exc) { | |
924 | ✗ | throw hpp::Error(exc.what()); | |
925 | } | ||
926 | } | ||
927 | |||
928 | // --------------------------------------------------------------- | ||
929 | |||
930 | ✗ | void Problem::createConfigurationConstraint(const char* constraintName, | |
931 | const hpp::floatSeq& goal, | ||
932 | const hpp::floatSeq& weights) { | ||
933 | try { | ||
934 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
935 | ✗ | std::string name(constraintName); | |
936 | ✗ | problemSolver()->numericalConstraints.add( | |
937 | name, | ||
938 | ✗ | Implicit::create(constraints::ConfigurationConstraint::create( | |
939 | ✗ | name, problemSolver()->robot(), | |
940 | ✗ | floatSeqToConfig(robot, goal, true), | |
941 | ✗ | floatSeqToVector(weights, robot->numberDof())), | |
942 | ✗ | 1 * constraints::EqualToZero)); | |
943 | ✗ | } catch (const std::exception& exc) { | |
944 | ✗ | throw hpp::Error(exc.what()); | |
945 | } | ||
946 | } | ||
947 | |||
948 | // --------------------------------------------------------------- | ||
949 | |||
950 | ✗ | void Problem::createDistanceBetweenJointConstraint(const char* constraintName, | |
951 | const char* joint1Name, | ||
952 | const char* joint2Name, | ||
953 | Double) { | ||
954 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
955 | try { | ||
956 | ✗ | JointPtr_t joint1 = problemSolver()->robot()->getJointByName(joint1Name); | |
957 | ✗ | JointPtr_t joint2 = problemSolver()->robot()->getJointByName(joint2Name); | |
958 | ✗ | std::string name(constraintName); | |
959 | ✗ | problemSolver()->addNumericalConstraint( | |
960 | name, | ||
961 | ✗ | Implicit::create(DistanceBetweenBodies::create( | |
962 | ✗ | name, problemSolver()->robot(), joint1, joint2), | |
963 | ✗ | 1 * constraints::EqualToZero)); | |
964 | ✗ | } catch (const std::exception& exc) { | |
965 | ✗ | throw Error(exc.what()); | |
966 | } | ||
967 | } | ||
968 | |||
969 | // --------------------------------------------------------------- | ||
970 | |||
971 | ✗ | void Problem::createDistanceBetweenJointAndObjects(const char* constraintName, | |
972 | const char* joint1Name, | ||
973 | const hpp::Names_t& objects, | ||
974 | Double) { | ||
975 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
976 | try { | ||
977 | ✗ | JointPtr_t joint1 = problemSolver()->robot()->getJointByName(joint1Name); | |
978 | ✗ | std::vector<CollisionObjectPtr_t> objectList; | |
979 | ✗ | for (CORBA::ULong i = 0; i < objects.length(); ++i) { | |
980 | ✗ | objectList.push_back(problemSolver()->obstacle(std::string(objects[i]))); | |
981 | } | ||
982 | ✗ | std::string name(constraintName); | |
983 | ✗ | problemSolver()->addNumericalConstraint( | |
984 | ✗ | name, Implicit::create( | |
985 | ✗ | DistanceBetweenBodies::create(name, problemSolver()->robot(), | |
986 | joint1, objectList), | ||
987 | ✗ | 1 * constraints::EqualToZero)); | |
988 | ✗ | } catch (const std::exception& exc) { | |
989 | ✗ | throw Error(exc.what()); | |
990 | } | ||
991 | } | ||
992 | |||
993 | // --------------------------------------------------------------- | ||
994 | |||
995 | ✗ | void Problem::createIdentityConstraint(const char* constraintName, | |
996 | const Names_t& inJoints, | ||
997 | const hpp::Names_t& outJoints) { | ||
998 | try { | ||
999 | using namespace constraints; | ||
1000 | |||
1001 | ✗ | DevicePtr_t robot = problemSolver()->robot(); | |
1002 | |||
1003 | ✗ | LiegroupSpacePtr_t ispace = LiegroupSpace::empty(); | |
1004 | ✗ | LiegroupSpacePtr_t ospace = LiegroupSpace::empty(); | |
1005 | ✗ | segments_t iq, oq, iv, ov; | |
1006 | ✗ | for (CORBA::ULong i = 0; i < inJoints.length(); ++i) { | |
1007 | ✗ | JointPtr_t j = robot->getJointByName(std::string(inJoints[i])); | |
1008 | ✗ | *ispace *= j->configurationSpace(); | |
1009 | ✗ | iq.push_back(segment_t(j->rankInConfiguration(), j->configSize())); | |
1010 | ✗ | iv.push_back(segment_t(j->rankInVelocity(), j->numberDof())); | |
1011 | } | ||
1012 | ✗ | for (CORBA::ULong i = 0; i < outJoints.length(); ++i) { | |
1013 | ✗ | JointPtr_t j = robot->getJointByName(std::string(outJoints[i])); | |
1014 | ✗ | *ospace *= j->configurationSpace(); | |
1015 | ✗ | oq.push_back(segment_t(j->rankInConfiguration(), j->configSize())); | |
1016 | ✗ | ov.push_back(segment_t(j->rankInVelocity(), j->numberDof())); | |
1017 | } | ||
1018 | ✗ | if (*ispace != *ospace) throw Error("Input and output space are different"); | |
1019 | ✗ | assert(BlockIndex::cardinal(iq) == BlockIndex::cardinal(oq)); | |
1020 | ✗ | assert(BlockIndex::cardinal(iv) == BlockIndex::cardinal(ov)); | |
1021 | |||
1022 | ✗ | problemSolver()->addNumericalConstraint( | |
1023 | constraintName, | ||
1024 | ✗ | Explicit::create(robot->configSpace(), | |
1025 | ✗ | Identity::create(ispace, constraintName), iq, oq, iv, | |
1026 | ov)); | ||
1027 | ✗ | } catch (std::exception& exc) { | |
1028 | ✗ | throw Error(exc.what()); | |
1029 | } | ||
1030 | } | ||
1031 | |||
1032 | // --------------------------------------------------------------- | ||
1033 | |||
1034 | ✗ | bool Problem::applyConstraints(const hpp::floatSeq& input, | |
1035 | hpp::floatSeq_out output, | ||
1036 | double& residualError) { | ||
1037 | ✗ | bool success = false; | |
1038 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1039 | try { | ||
1040 | ✗ | Configuration_t config = floatSeqToConfig(robot, input, true); | |
1041 | ✗ | success = problemSolver()->constraints()->apply(config); | |
1042 | ✗ | if (hpp::core::ConfigProjectorPtr_t configProjector = | |
1043 | ✗ | problemSolver()->constraints()->configProjector()) { | |
1044 | ✗ | residualError = configProjector->residualError(); | |
1045 | } | ||
1046 | ✗ | output = vectorToFloatSeq(config); | |
1047 | ✗ | } catch (const std::exception& exc) { | |
1048 | ✗ | throw hpp::Error(exc.what()); | |
1049 | } | ||
1050 | ✗ | return success; | |
1051 | } | ||
1052 | |||
1053 | // --------------------------------------------------------------- | ||
1054 | |||
1055 | ✗ | bool Problem::optimize(const hpp::floatSeq& input, hpp::floatSeq_out output, | |
1056 | hpp::floatSeq_out residualError) { | ||
1057 | ✗ | bool success = false; | |
1058 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1059 | ✗ | Configuration_t config = floatSeqToConfig(robot, input, true); | |
1060 | ✗ | vector_t err; | |
1061 | try { | ||
1062 | ✗ | if (!problemSolver()->constraints()) | |
1063 | ✗ | throw hpp::Error("The problem has no constraints"); | |
1064 | ✗ | if (!problemSolver()->constraints()->configProjector()) | |
1065 | ✗ | throw hpp::Error("The problem has no config projector"); | |
1066 | core::ConfigProjectorPtr_t cp = | ||
1067 | ✗ | problemSolver()->constraints()->configProjector(); | |
1068 | |||
1069 | ✗ | success = cp->optimize(config, 0); | |
1070 | ✗ | cp->isSatisfied(config, err); | |
1071 | |||
1072 | ✗ | } catch (const std::exception& exc) { | |
1073 | ✗ | throw hpp::Error(exc.what()); | |
1074 | } | ||
1075 | ✗ | output = vectorToFloatSeq(config); | |
1076 | ✗ | residualError = vectorToFloatSeq(err); | |
1077 | ✗ | return success; | |
1078 | } | ||
1079 | |||
1080 | // --------------------------------------------------------------- | ||
1081 | |||
1082 | ✗ | void Problem::computeValueAndJacobian(const hpp::floatSeq& config, | |
1083 | hpp::floatSeq_out value, | ||
1084 | hpp::floatSeqSeq_out jacobian) { | ||
1085 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1086 | try { | ||
1087 | ✗ | Configuration_t configuration = floatSeqToConfig(robot, config, true); | |
1088 | ✗ | vector_t v; | |
1089 | ✗ | matrix_t J; | |
1090 | ✗ | problemSolver()->computeValueAndJacobian(configuration, v, J); | |
1091 | ✗ | value = vectorToFloatSeq(v); | |
1092 | ✗ | jacobian = matrixToFloatSeqSeq(J); | |
1093 | ✗ | } catch (const std::exception& exc) { | |
1094 | ✗ | throw hpp::Error(exc.what()); | |
1095 | } | ||
1096 | } | ||
1097 | |||
1098 | // --------------------------------------------------------------- | ||
1099 | |||
1100 | ✗ | bool Problem::generateValidConfig(ULong maxIter, hpp::floatSeq_out output, | |
1101 | double& residualError) { | ||
1102 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
1103 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
1104 | ✗ | if (!ps->problem()) ps->resetProblem(); | |
1105 | core::ConfigurationShooterPtr_t shooter = | ||
1106 | ✗ | ps->problem()->configurationShooter(); | |
1107 | ✗ | bool success = false, configIsValid = false; | |
1108 | ✗ | Configuration_t config; | |
1109 | ✗ | while (!configIsValid && maxIter > 0) { | |
1110 | try { | ||
1111 | ✗ | config = shooter->shoot(); | |
1112 | ✗ | success = ps->constraints()->apply(config); | |
1113 | ✗ | if (hpp::core::ConfigProjectorPtr_t configProjector = | |
1114 | ✗ | ps->constraints()->configProjector()) { | |
1115 | ✗ | residualError = configProjector->residualError(); | |
1116 | } | ||
1117 | ✗ | if (success) { | |
1118 | ✗ | core::ValidationReportPtr_t validationReport; | |
1119 | ✗ | configIsValid = ps->problem()->configValidations()->validate( | |
1120 | config, validationReport); | ||
1121 | } | ||
1122 | ✗ | } catch (const std::exception& exc) { | |
1123 | ✗ | throw hpp::Error(exc.what()); | |
1124 | } | ||
1125 | ✗ | maxIter--; | |
1126 | } | ||
1127 | ✗ | output = vectorToFloatSeq(config); | |
1128 | ✗ | return configIsValid; | |
1129 | } | ||
1130 | |||
1131 | // --------------------------------------------------------------- | ||
1132 | |||
1133 | ✗ | void Problem::resetConstraintMap() { | |
1134 | try { | ||
1135 | ✗ | problemSolver()->numericalConstraints.clear(); | |
1136 | ✗ | } catch (const std::exception& exc) { | |
1137 | ✗ | throw hpp::Error(exc.what()); | |
1138 | } | ||
1139 | } | ||
1140 | |||
1141 | // --------------------------------------------------------------- | ||
1142 | |||
1143 | ✗ | void Problem::resetConstraints() { | |
1144 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
1145 | try { | ||
1146 | ✗ | problemSolver()->resetConstraints(); | |
1147 | ✗ | } catch (const std::exception& exc) { | |
1148 | ✗ | throw hpp::Error(exc.what()); | |
1149 | } | ||
1150 | } | ||
1151 | |||
1152 | // --------------------------------------------------------------- | ||
1153 | |||
1154 | ✗ | void Problem::filterCollisionPairs() { | |
1155 | try { | ||
1156 | ✗ | problemSolver()->filterCollisionPairs(); | |
1157 | ✗ | } catch (const std::exception& exc) { | |
1158 | ✗ | throw hpp::Error(exc.what()); | |
1159 | } | ||
1160 | } | ||
1161 | |||
1162 | // --------------------------------------------------------------- | ||
1163 | |||
1164 | ✗ | void Problem::addPassiveDofs(const char* passiveDofsName, | |
1165 | const hpp::Names_t& dofNames) { | ||
1166 | ✗ | DevicePtr_t robot = problemSolver()->robot(); | |
1167 | ✗ | if (!robot) throw hpp::Error("No robot loaded"); | |
1168 | ✗ | std::vector<size_type> dofs; | |
1169 | /// First, translate names into velocity indexes. | ||
1170 | ✗ | for (CORBA::ULong i = 0; i < dofNames.length(); ++i) { | |
1171 | ✗ | std::string name(dofNames[i]); | |
1172 | ✗ | JointPtr_t j = robot->getJointByName(name); | |
1173 | ✗ | if (!j) { | |
1174 | ✗ | std::ostringstream oss; | |
1175 | ✗ | oss << "Joint " << name << " not found."; | |
1176 | ✗ | throw hpp::Error(oss.str().c_str()); | |
1177 | } | ||
1178 | ✗ | for (size_type i = 0; i < j->numberDof(); i++) | |
1179 | ✗ | dofs.push_back(j->rankInVelocity() + i); | |
1180 | } | ||
1181 | /// Then, sort and remove non duplicated elements. | ||
1182 | ✗ | std::sort(dofs.begin(), dofs.end()); | |
1183 | ✗ | std::vector<size_type>::iterator last = std::unique(dofs.begin(), dofs.end()); | |
1184 | ✗ | dofs.erase(last, dofs.end()); | |
1185 | /// Then, create the intervals. | ||
1186 | ✗ | core::segments_t passiveDofs; | |
1187 | ✗ | dofs.push_back(robot->numberDof() + 1); | |
1188 | ✗ | size_type intStart = dofs[0], intEnd = dofs[0]; | |
1189 | ✗ | for (size_t i = 1; i < dofs.size(); i++) { | |
1190 | ✗ | intEnd++; | |
1191 | ✗ | if (intEnd == dofs[i]) { | |
1192 | ✗ | continue; | |
1193 | } else { | ||
1194 | ✗ | passiveDofs.push_back(core::segment_t(intStart, intEnd - intStart)); | |
1195 | ✗ | intStart = intEnd = dofs[i]; | |
1196 | } | ||
1197 | } | ||
1198 | ✗ | problemSolver()->passiveDofs.add(passiveDofsName, passiveDofs); | |
1199 | } | ||
1200 | |||
1201 | // --------------------------------------------------------------- | ||
1202 | |||
1203 | ✗ | void Problem::getConstraintDimensions(const char* constraintName, | |
1204 | ULong& inputSize, | ||
1205 | ULong& inputDerivativeSize, | ||
1206 | ULong& outputSize, | ||
1207 | ULong& outputDerivativeSize) { | ||
1208 | try { | ||
1209 | ✗ | std::string n(constraintName); | |
1210 | ✗ | if (!problemSolver()->numericalConstraints.has(n)) | |
1211 | ✗ | throw Error(("Constraint " + n + " not found").c_str()); | |
1212 | ✗ | ImplicitPtr_t c = problemSolver()->numericalConstraints.get(n); | |
1213 | ✗ | inputSize = (ULong)c->function().inputSize(); | |
1214 | ✗ | inputDerivativeSize = (ULong)c->function().inputDerivativeSize(); | |
1215 | ✗ | outputSize = (ULong)c->function().outputSize(); | |
1216 | ✗ | outputDerivativeSize = (ULong)c->function().outputDerivativeSize(); | |
1217 | ✗ | } catch (const std::exception& e) { | |
1218 | ✗ | throw hpp::Error(e.what()); | |
1219 | } | ||
1220 | } | ||
1221 | |||
1222 | // --------------------------------------------------------------- | ||
1223 | |||
1224 | ✗ | void Problem::setConstantRightHandSide(const char* constraintName, | |
1225 | CORBA::Boolean constant) { | ||
1226 | try { | ||
1227 | ✗ | if (constant) { | |
1228 | ✗ | problemSolver()->comparisonType(constraintName, constraints::EqualToZero); | |
1229 | } else { | ||
1230 | ✗ | problemSolver()->comparisonType(constraintName, constraints::Equality); | |
1231 | } | ||
1232 | ✗ | } catch (const std::exception& exc) { | |
1233 | ✗ | throw hpp::Error(exc.what()); | |
1234 | } | ||
1235 | } | ||
1236 | |||
1237 | // --------------------------------------------------------------- | ||
1238 | |||
1239 | ✗ | bool Problem::getConstantRightHandSide(const char* constraintName) { | |
1240 | try { | ||
1241 | ✗ | ImplicitPtr_t nc(problemSolver()->numericalConstraints.get(constraintName)); | |
1242 | ✗ | return nc->parameterSize() == 0; | |
1243 | ✗ | } catch (const std::exception& exc) { | |
1244 | ✗ | throw hpp::Error(exc.what()); | |
1245 | } | ||
1246 | } | ||
1247 | |||
1248 | // --------------------------------------------------------------- | ||
1249 | |||
1250 | ✗ | void Problem::setRightHandSide(const hpp::floatSeq& rhs) { | |
1251 | try { | ||
1252 | hpp::core::ConfigProjectorPtr_t configProjector( | ||
1253 | ✗ | problemSolver()->constraints()->configProjector()); | |
1254 | ✗ | if (!configProjector) { | |
1255 | ✗ | throw std::runtime_error("No constraint has been set."); | |
1256 | } | ||
1257 | ✗ | vector_t rightHandSide(floatSeqToVector(rhs)); | |
1258 | ✗ | configProjector->rightHandSide(rightHandSide); | |
1259 | ✗ | } catch (const std::exception& exc) { | |
1260 | ✗ | throw hpp::Error(exc.what()); | |
1261 | } | ||
1262 | } | ||
1263 | |||
1264 | // --------------------------------------------------------------- | ||
1265 | |||
1266 | ✗ | void Problem::setRightHandSideFromConfig(const hpp::floatSeq& config) { | |
1267 | try { | ||
1268 | hpp::core::ConfigProjectorPtr_t configProjector( | ||
1269 | ✗ | problemSolver()->constraints()->configProjector()); | |
1270 | ✗ | if (!configProjector) { | |
1271 | ✗ | throw std::runtime_error("No constraint has been set."); | |
1272 | } | ||
1273 | ✗ | Configuration_t q(floatSeqToConfig(problemSolver()->robot(), config, true)); | |
1274 | ✗ | configProjector->rightHandSideFromConfig(q); | |
1275 | ✗ | } catch (const std::exception& exc) { | |
1276 | ✗ | throw hpp::Error(exc.what()); | |
1277 | } | ||
1278 | } | ||
1279 | |||
1280 | // --------------------------------------------------------------- | ||
1281 | |||
1282 | ✗ | void Problem::setRightHandSideFromConfigByName(const char* constraintName, | |
1283 | const hpp::floatSeq& config) { | ||
1284 | try { | ||
1285 | hpp::core::ConfigProjectorPtr_t configProjector( | ||
1286 | ✗ | problemSolver()->constraints()->configProjector()); | |
1287 | ✗ | if (!configProjector) { | |
1288 | ✗ | throw std::runtime_error("No constraint has been set."); | |
1289 | } | ||
1290 | ✗ | Configuration_t q(floatSeqToConfig(problemSolver()->robot(), config, true)); | |
1291 | // look for constraint with this key | ||
1292 | ✗ | if (problemSolver()->numericalConstraints.has(constraintName)) { | |
1293 | ImplicitPtr_t nc( | ||
1294 | ✗ | problemSolver()->numericalConstraints.get(constraintName)); | |
1295 | ✗ | configProjector->rightHandSideFromConfig(nc, q); | |
1296 | ✗ | return; | |
1297 | } | ||
1298 | std::string msg( | ||
1299 | "Config projector does not contain any numerical" | ||
1300 | ✗ | " constraint with name "); | |
1301 | ✗ | msg += constraintName; | |
1302 | ✗ | throw std::runtime_error(msg.c_str()); | |
1303 | ✗ | } catch (const std::exception& exc) { | |
1304 | ✗ | throw hpp::Error(exc.what()); | |
1305 | } | ||
1306 | } | ||
1307 | |||
1308 | // --------------------------------------------------------------- | ||
1309 | |||
1310 | ✗ | void Problem::setRightHandSideByName(const char* constraintName, | |
1311 | const hpp::floatSeq& rhs) { | ||
1312 | try { | ||
1313 | hpp::core::ConfigProjectorPtr_t configProjector( | ||
1314 | ✗ | problemSolver()->constraints()->configProjector()); | |
1315 | ✗ | if (!configProjector) { | |
1316 | ✗ | throw std::runtime_error("No constraint has been set."); | |
1317 | } | ||
1318 | ✗ | vector_t rightHandSide(floatSeqToVector(rhs)); | |
1319 | // look for constraint with this key | ||
1320 | ✗ | if (problemSolver()->numericalConstraints.has(constraintName)) { | |
1321 | ImplicitPtr_t nc( | ||
1322 | ✗ | problemSolver()->numericalConstraints.get(constraintName)); | |
1323 | ✗ | configProjector->rightHandSide(nc, rightHandSide); | |
1324 | ✗ | return; | |
1325 | } | ||
1326 | std::string msg( | ||
1327 | "Config projector does not contain any numerical" | ||
1328 | ✗ | " constraint with name "); | |
1329 | ✗ | msg += constraintName; | |
1330 | ✗ | throw std::runtime_error(msg.c_str()); | |
1331 | ✗ | } catch (const std::exception& exc) { | |
1332 | ✗ | throw hpp::Error(exc.what()); | |
1333 | } | ||
1334 | } | ||
1335 | |||
1336 | // --------------------------------------------------------------- | ||
1337 | |||
1338 | ✗ | hpp::floatSeq* Problem::getRightHandSide() { | |
1339 | try { | ||
1340 | hpp::core::ConfigProjectorPtr_t configProjector( | ||
1341 | ✗ | problemSolver()->constraints()->configProjector()); | |
1342 | ✗ | if (!configProjector) { | |
1343 | ✗ | throw std::runtime_error("No constraint has been set."); | |
1344 | } | ||
1345 | ✗ | return vectorToFloatSeq(configProjector->rightHandSide()); | |
1346 | ✗ | } catch (const std::exception& exc) { | |
1347 | ✗ | throw hpp::Error(exc.what()); | |
1348 | } | ||
1349 | } | ||
1350 | |||
1351 | // --------------------------------------------------------------- | ||
1352 | |||
1353 | ✗ | void Problem::addNumericalConstraints(const char* configProjName, | |
1354 | const Names_t& constraintNames, | ||
1355 | const hpp::intSeq& priorities) { | ||
1356 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
1357 | try { | ||
1358 | ✗ | for (CORBA::ULong i = 0; i < constraintNames.length(); ++i) { | |
1359 | ✗ | std::string name(constraintNames[i]); | |
1360 | ✗ | problemSolver()->addNumericalConstraintToConfigProjector( | |
1361 | ✗ | configProjName, name, (std::size_t)priorities[i]); | |
1362 | } | ||
1363 | ✗ | } catch (const std::exception& exc) { | |
1364 | ✗ | throw Error(exc.what()); | |
1365 | } | ||
1366 | } | ||
1367 | |||
1368 | // --------------------------------------------------------------- | ||
1369 | |||
1370 | ✗ | void Problem::setNumericalConstraintsLastPriorityOptional(const bool optional) { | |
1371 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
1372 | ✗ | if (!problemSolver()->constraints()) | |
1373 | ✗ | throw hpp::Error("The problem has no constraints"); | |
1374 | ✗ | if (!problemSolver()->constraints()->configProjector()) | |
1375 | ✗ | throw hpp::Error("The problem has no config projector"); | |
1376 | try { | ||
1377 | ✗ | problemSolver()->constraints()->configProjector()->lastIsOptional(optional); | |
1378 | ✗ | } catch (const std::exception& exc) { | |
1379 | ✗ | throw Error(exc.what()); | |
1380 | } | ||
1381 | } | ||
1382 | |||
1383 | // --------------------------------------------------------------- | ||
1384 | |||
1385 | ✗ | void Problem::addLockedJointConstraints(const char* configProjName, | |
1386 | const hpp::Names_t& lockedJointNames) { | ||
1387 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
1388 | try { | ||
1389 | ✗ | for (CORBA::ULong i = 0; i < lockedJointNames.length(); ++i) { | |
1390 | ✗ | std::string name(lockedJointNames[i]); | |
1391 | ✗ | problemSolver()->addNumericalConstraintToConfigProjector(configProjName, | |
1392 | name); | ||
1393 | } | ||
1394 | ✗ | } catch (const std::exception& exc) { | |
1395 | ✗ | throw Error(exc.what()); | |
1396 | } | ||
1397 | } | ||
1398 | |||
1399 | // --------------------------------------------------------------- | ||
1400 | |||
1401 | ✗ | char* Problem::displayConstraints() { | |
1402 | try { | ||
1403 | ✗ | std::ostringstream oss; | |
1404 | ✗ | if (problemSolver()->constraints()) oss << *problemSolver()->constraints(); | |
1405 | ✗ | return c_str(oss.str()); | |
1406 | ✗ | } catch (const std::exception& exc) { | |
1407 | ✗ | throw hpp::Error(exc.what()); | |
1408 | } | ||
1409 | } | ||
1410 | |||
1411 | // --------------------------------------------------------------- | ||
1412 | |||
1413 | ✗ | Double Problem::getErrorThreshold() { | |
1414 | ✗ | return problemSolver()->errorThreshold(); | |
1415 | } | ||
1416 | |||
1417 | // --------------------------------------------------------------- | ||
1418 | |||
1419 | typedef std::map<std::string, core::ConfigProjector::LineSearchType> | ||
1420 | LineSearchTypeMap_t; | ||
1421 | |||
1422 | ✗ | LineSearchTypeMap_t makeLineSearchTypeMap() { | |
1423 | ✗ | LineSearchTypeMap_t map; | |
1424 | ✗ | map["Backtracking"] = core::ConfigProjector::Backtracking; | |
1425 | ✗ | map["FixedSequence"] = core::ConfigProjector::FixedSequence; | |
1426 | ✗ | map["Constant"] = core::ConfigProjector::Constant; | |
1427 | ✗ | map["ErrorNormBased"] = core::ConfigProjector::ErrorNormBased; | |
1428 | ✗ | return map; | |
1429 | } | ||
1430 | ✗ | void Problem::setDefaultLineSearchType(const char* type) { | |
1431 | ✗ | static const LineSearchTypeMap_t map = makeLineSearchTypeMap(); | |
1432 | ✗ | std::string t(type); | |
1433 | |||
1434 | ✗ | LineSearchTypeMap_t::const_iterator _t = map.find(t); | |
1435 | ✗ | if (_t != map.end()) | |
1436 | ✗ | core::ConfigProjector::defaultLineSearch(_t->second); | |
1437 | else { | ||
1438 | ✗ | std::ostringstream oss; | |
1439 | ✗ | oss << "Available types are:"; | |
1440 | ✗ | for (_t = map.begin(); _t != map.end(); ++_t) | |
1441 | ✗ | oss << " \"" << _t->first << '"'; | |
1442 | ✗ | throw Error(oss.str().c_str()); | |
1443 | } | ||
1444 | } | ||
1445 | |||
1446 | // --------------------------------------------------------------- | ||
1447 | |||
1448 | ✗ | void Problem::setErrorThreshold(Double threshold) { | |
1449 | ✗ | problemSolver()->errorThreshold(threshold); | |
1450 | } | ||
1451 | |||
1452 | // --------------------------------------------------------------- | ||
1453 | ✗ | void Problem::setMaxIterProjection(ULong iterations) { | |
1454 | ✗ | problemSolver()->maxIterProjection((size_type)iterations); | |
1455 | } | ||
1456 | |||
1457 | // --------------------------------------------------------------- | ||
1458 | ✗ | ULong Problem::getMaxIterProjection() { | |
1459 | ✗ | return (ULong)problemSolver()->maxIterProjection(); | |
1460 | } | ||
1461 | |||
1462 | // --------------------------------------------------------------- | ||
1463 | ✗ | void Problem::setMaxIterPathPlanning(ULong iterations) { | |
1464 | ✗ | problemSolver()->maxIterPathPlanning((size_type)iterations); | |
1465 | } | ||
1466 | |||
1467 | // --------------------------------------------------------------- | ||
1468 | ✗ | ULong Problem::getMaxIterPathPlanning() { | |
1469 | ✗ | return (ULong)problemSolver()->maxIterPathPlanning(); | |
1470 | } | ||
1471 | |||
1472 | // --------------------------------------------------------------- | ||
1473 | ✗ | void Problem::setTimeOutPathPlanning(double timeOut) { | |
1474 | ✗ | problemSolver()->setTimeOutPathPlanning(timeOut); | |
1475 | } | ||
1476 | |||
1477 | // --------------------------------------------------------------- | ||
1478 | ✗ | double Problem::getTimeOutPathPlanning() { | |
1479 | ✗ | return problemSolver()->getTimeOutPathPlanning(); | |
1480 | } | ||
1481 | |||
1482 | // --------------------------------------------------------------- | ||
1483 | |||
1484 | ✗ | void Problem::selectPathPlanner(const char* pathPlannerType) { | |
1485 | try { | ||
1486 | ✗ | problemSolver()->pathPlannerType(std::string(pathPlannerType)); | |
1487 | ✗ | } catch (const std::exception& exc) { | |
1488 | ✗ | throw hpp::Error(exc.what()); | |
1489 | } | ||
1490 | } | ||
1491 | |||
1492 | // --------------------------------------------------------------- | ||
1493 | |||
1494 | ✗ | void Problem::selectConfigurationShooter(const char* configurationShooterType) { | |
1495 | try { | ||
1496 | ✗ | problemSolver()->configurationShooterType( | |
1497 | ✗ | std::string(configurationShooterType)); | |
1498 | ✗ | } catch (const std::exception& exc) { | |
1499 | ✗ | throw hpp::Error(exc.what()); | |
1500 | } | ||
1501 | } | ||
1502 | |||
1503 | // --------------------------------------------------------------- | ||
1504 | |||
1505 | ✗ | void Problem::selectDistance(const char* distanceType) { | |
1506 | try { | ||
1507 | ✗ | problemSolver()->distanceType(std::string(distanceType)); | |
1508 | ✗ | } catch (const std::exception& exc) { | |
1509 | ✗ | throw hpp::Error(exc.what()); | |
1510 | } | ||
1511 | } | ||
1512 | |||
1513 | // --------------------------------------------------------------- | ||
1514 | |||
1515 | ✗ | void Problem::selectSteeringMethod(const char* steeringMethodType) { | |
1516 | try { | ||
1517 | ✗ | problemSolver()->steeringMethodType(std::string(steeringMethodType)); | |
1518 | ✗ | } catch (const std::exception& exc) { | |
1519 | ✗ | throw hpp::Error(exc.what()); | |
1520 | } | ||
1521 | } | ||
1522 | |||
1523 | // --------------------------------------------------------------- | ||
1524 | ✗ | void Problem::addPathOptimizer(const char* pathOptimizerType) { | |
1525 | try { | ||
1526 | ✗ | problemSolver()->addPathOptimizer(std::string(pathOptimizerType)); | |
1527 | ✗ | } catch (const std::exception& exc) { | |
1528 | ✗ | throw hpp::Error(exc.what()); | |
1529 | } | ||
1530 | } | ||
1531 | |||
1532 | // --------------------------------------------------------------- | ||
1533 | |||
1534 | ✗ | void Problem::clearPathOptimizers() { | |
1535 | try { | ||
1536 | ✗ | problemSolver()->clearPathOptimizers(); | |
1537 | ✗ | } catch (const std::exception& exc) { | |
1538 | ✗ | throw hpp::Error(exc.what()); | |
1539 | } | ||
1540 | } | ||
1541 | |||
1542 | // --------------------------------------------------------------- | ||
1543 | ✗ | void Problem::addConfigValidation(const char* configValidationType) { | |
1544 | try { | ||
1545 | ✗ | problemSolver()->addConfigValidation(std::string(configValidationType)); | |
1546 | ✗ | } catch (const std::exception& exc) { | |
1547 | ✗ | throw hpp::Error(exc.what()); | |
1548 | } | ||
1549 | } | ||
1550 | |||
1551 | // --------------------------------------------------------------- | ||
1552 | |||
1553 | ✗ | void Problem::clearConfigValidations() { | |
1554 | try { | ||
1555 | ✗ | problemSolver()->clearConfigValidations(); | |
1556 | ✗ | } catch (const std::exception& exc) { | |
1557 | ✗ | throw hpp::Error(exc.what()); | |
1558 | } | ||
1559 | } | ||
1560 | |||
1561 | // --------------------------------------------------------------- | ||
1562 | |||
1563 | ✗ | void Problem::selectPathValidation(const char* pathValidationType, | |
1564 | Double tolerance) { | ||
1565 | try { | ||
1566 | ✗ | problemSolver()->pathValidationType(std::string(pathValidationType), | |
1567 | tolerance); | ||
1568 | ✗ | } catch (const std::exception& exc) { | |
1569 | ✗ | throw hpp::Error(exc.what()); | |
1570 | } | ||
1571 | } | ||
1572 | |||
1573 | // --------------------------------------------------------------- | ||
1574 | |||
1575 | ✗ | void Problem::selectPathProjector(const char* pathProjectorType, | |
1576 | Double tolerance) { | ||
1577 | try { | ||
1578 | ✗ | problemSolver()->pathProjectorType(std::string(pathProjectorType), | |
1579 | tolerance); | ||
1580 | ✗ | } catch (const std::exception& exc) { | |
1581 | ✗ | throw hpp::Error(exc.what()); | |
1582 | } | ||
1583 | } | ||
1584 | |||
1585 | // --------------------------------------------------------------- | ||
1586 | |||
1587 | ✗ | bool Problem::prepareSolveStepByStep() { | |
1588 | try { | ||
1589 | ✗ | return problemSolver()->prepareSolveStepByStep(); | |
1590 | ✗ | } catch (const std::exception& exc) { | |
1591 | ✗ | throw hpp::Error(exc.what()); | |
1592 | } | ||
1593 | return false; | ||
1594 | } | ||
1595 | |||
1596 | // --------------------------------------------------------------- | ||
1597 | |||
1598 | ✗ | bool Problem::executeOneStep() { | |
1599 | try { | ||
1600 | ✗ | return problemSolver()->executeOneStep(); | |
1601 | ✗ | } catch (const std::exception& exc) { | |
1602 | ✗ | throw hpp::Error(exc.what()); | |
1603 | } | ||
1604 | return false; | ||
1605 | } | ||
1606 | |||
1607 | // --------------------------------------------------------------- | ||
1608 | |||
1609 | ✗ | void Problem::finishSolveStepByStep() { | |
1610 | try { | ||
1611 | ✗ | problemSolver()->finishSolveStepByStep(); | |
1612 | ✗ | } catch (const std::exception& exc) { | |
1613 | ✗ | throw hpp::Error(exc.what()); | |
1614 | } | ||
1615 | } | ||
1616 | |||
1617 | // --------------------------------------------------------------- | ||
1618 | |||
1619 | ✗ | hpp::intSeq* Problem::solve() { | |
1620 | try { | ||
1621 | boost::posix_time::ptime start = | ||
1622 | ✗ | boost::posix_time::microsec_clock::universal_time(); | |
1623 | ✗ | problemSolver()->solve(); | |
1624 | boost::posix_time::time_duration time = | ||
1625 | ✗ | boost::posix_time::microsec_clock::universal_time() - start; | |
1626 | ✗ | hpp::intSeq* ret = new hpp::intSeq; | |
1627 | ✗ | ret->length(4); | |
1628 | ✗ | (*ret)[0] = static_cast<CORBA::Long>(time.hours()); | |
1629 | ✗ | (*ret)[1] = static_cast<CORBA::Long>(time.minutes()); | |
1630 | ✗ | (*ret)[2] = static_cast<CORBA::Long>(time.seconds()); | |
1631 | ✗ | (*ret)[3] = static_cast<CORBA::Long>(time.fractional_seconds() / 1000); | |
1632 | ✗ | return ret; | |
1633 | ✗ | } catch (const std::exception& exc) { | |
1634 | ✗ | throw hpp::Error(exc.what()); | |
1635 | } | ||
1636 | } | ||
1637 | |||
1638 | // --------------------------------------------------------------- | ||
1639 | |||
1640 | 1 | bool Problem::directPath(const hpp::floatSeq& startConfig, | |
1641 | const hpp::floatSeq& endConfig, | ||
1642 | CORBA::Boolean validate, ULong& pathId, | ||
1643 | CORBA::String_out report) { | ||
1644 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Configuration_t start; |
1645 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Configuration_t end; |
1646 | 1 | bool pathValid = false; | |
1647 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
1648 | try { | ||
1649 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | start = floatSeqToConfig(robot, startConfig, true); |
1650 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end = floatSeqToConfig(robot, endConfig, true); |
1651 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
|
1 | if (!problemSolver()->problem()) { |
1652 | ✗ | problemSolver()->resetProblem(); | |
1653 | } | ||
1654 | std::size_t pid; | ||
1655 | 1 | std::string r; | |
1656 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | pathValid = problemSolver()->directPath(start, end, validate, pid, r); |
1657 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | report = CORBA::string_dup(r.c_str()); |
1658 | 1 | pathId = (ULong)pid; | |
1659 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | } catch (const std::exception& exc) { |
1660 | ✗ | throw hpp::Error(exc.what()); | |
1661 | } | ||
1662 | 1 | return pathValid; | |
1663 | 1 | } | |
1664 | |||
1665 | // --------------------------------------------------------------- | ||
1666 | |||
1667 | ✗ | bool Problem::reversePath(ULong pathId, ULong& reversedPathId) { | |
1668 | try { | ||
1669 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1670 | ✗ | std::ostringstream oss("wrong path id. "); | |
1671 | ✗ | oss << "Number path: " << problemSolver()->paths().size() << "."; | |
1672 | ✗ | std::string err = oss.str(); | |
1673 | ✗ | throw hpp::Error(err.c_str()); | |
1674 | } | ||
1675 | ✗ | PathVectorPtr_t path = problemSolver()->paths()[pathId]; | |
1676 | PathVectorPtr_t reversed = | ||
1677 | ✗ | HPP_DYNAMIC_PTR_CAST(core::PathVector, path->reverse()); | |
1678 | ✗ | if (!reversed) return false; | |
1679 | ✗ | problemSolver()->addPath(reversed); | |
1680 | ✗ | reversedPathId = (ULong)problemSolver()->paths().size() - 1; | |
1681 | ✗ | } catch (const std::exception& exc) { | |
1682 | ✗ | throw hpp::Error(exc.what()); | |
1683 | } | ||
1684 | ✗ | return true; | |
1685 | } | ||
1686 | |||
1687 | // --------------------------------------------------------------- | ||
1688 | |||
1689 | ✗ | void Problem::addConfigToRoadmap(const hpp::floatSeq& config) { | |
1690 | try { | ||
1691 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1692 | ✗ | Configuration_t configuration(floatSeqToConfig(robot, config, true)); | |
1693 | ✗ | problemSolver()->addConfigToRoadmap(configuration); | |
1694 | ✗ | } catch (const std::exception& exc) { | |
1695 | ✗ | throw hpp::Error(exc.what()); | |
1696 | } | ||
1697 | } | ||
1698 | |||
1699 | // --------------------------------------------------------------- | ||
1700 | |||
1701 | ✗ | void Problem::addEdgeToRoadmap(const hpp::floatSeq& config1, | |
1702 | const hpp::floatSeq& config2, ULong pathId, | ||
1703 | bool bothEdges) { | ||
1704 | try { | ||
1705 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1706 | ✗ | std::ostringstream oss("wrong path id: "); | |
1707 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1708 | ✗ | << "."; | |
1709 | ✗ | throw std::runtime_error(oss.str()); | |
1710 | } | ||
1711 | ✗ | PathVectorPtr_t path = problemSolver()->paths()[pathId]; | |
1712 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1713 | ✗ | Configuration_t start(floatSeqToConfig(robot, config1, true)); | |
1714 | ✗ | Configuration_t finish(floatSeqToConfig(robot, config2, true)); | |
1715 | ✗ | if (bothEdges) { | |
1716 | ✗ | problemSolver()->addEdgeToRoadmap(start, finish, path); | |
1717 | ✗ | problemSolver()->addEdgeToRoadmap(finish, start, path->reverse()); | |
1718 | ✗ | return; | |
1719 | } | ||
1720 | ✗ | problemSolver()->addEdgeToRoadmap(start, finish, path); | |
1721 | ✗ | return; | |
1722 | ✗ | } catch (const std::exception& exc) { | |
1723 | ✗ | throw hpp::Error(exc.what()); | |
1724 | } | ||
1725 | } | ||
1726 | // --------------------------------------------------------------- | ||
1727 | |||
1728 | ✗ | void Problem::appendDirectPath(ULong pathId, const hpp::floatSeq& config, | |
1729 | Boolean validate) { | ||
1730 | try { | ||
1731 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1732 | ✗ | std::ostringstream oss("wrong path id: "); | |
1733 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1734 | ✗ | << "."; | |
1735 | ✗ | throw std::runtime_error(oss.str()); | |
1736 | } | ||
1737 | ✗ | PathVectorPtr_t path = problemSolver()->paths()[pathId]; | |
1738 | ✗ | Configuration_t start(path->end()); | |
1739 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
1740 | ✗ | Configuration_t end(floatSeqToConfig(robot, config, true)); | |
1741 | ✗ | if (!problemSolver()->problem()) { | |
1742 | ✗ | problemSolver()->resetProblem(); | |
1743 | } | ||
1744 | ✗ | SteeringMethodPtr_t sm(problemSolver()->problem()->steeringMethod()); | |
1745 | ✗ | PathPtr_t dp = (*sm)(start, end); | |
1746 | ✗ | if (!dp) { | |
1747 | ✗ | std::ostringstream oss; | |
1748 | oss << "steering method failed to build a path between q1=" | ||
1749 | ✗ | << pinocchio::displayConfig(start) | |
1750 | ✗ | << "; q2=" << pinocchio::displayConfig(end) << "."; | |
1751 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
1752 | } | ||
1753 | ✗ | if (validate) { | |
1754 | ✗ | PathPtr_t unused; | |
1755 | ✗ | PathValidationReportPtr_t report; | |
1756 | ✗ | if (!problemSolver()->problem()->pathValidation()->validate( | |
1757 | dp, false, unused, report)) { | ||
1758 | ✗ | std::ostringstream oss; | |
1759 | ✗ | oss << *report; | |
1760 | ✗ | throw hpp::Error(oss.str().c_str()); | |
1761 | } | ||
1762 | } | ||
1763 | ✗ | path->appendPath(dp); | |
1764 | ✗ | } catch (const std::exception& exc) { | |
1765 | ✗ | throw hpp::Error(exc.what()); | |
1766 | } | ||
1767 | } | ||
1768 | |||
1769 | // --------------------------------------------------------------- | ||
1770 | |||
1771 | ✗ | void Problem::concatenatePath(ULong startId, ULong endId) { | |
1772 | try { | ||
1773 | ✗ | if (startId >= problemSolver()->paths().size() || | |
1774 | ✗ | endId >= problemSolver()->paths().size()) { | |
1775 | ✗ | std::ostringstream oss("wrong path id. "); | |
1776 | ✗ | oss << "Number path: " << problemSolver()->paths().size() << "."; | |
1777 | ✗ | throw std::runtime_error(oss.str()); | |
1778 | } | ||
1779 | ✗ | PathVectorPtr_t start = problemSolver()->paths()[startId]; | |
1780 | ✗ | PathVectorPtr_t end = problemSolver()->paths()[endId]; | |
1781 | ✗ | start->concatenate(end); | |
1782 | ✗ | } catch (const std::exception& exc) { | |
1783 | ✗ | throw hpp::Error(exc.what()); | |
1784 | } | ||
1785 | } | ||
1786 | |||
1787 | // --------------------------------------------------------------- | ||
1788 | |||
1789 | ✗ | void Problem::extractPath(ULong pathId, Double start, Double end) { | |
1790 | try { | ||
1791 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1792 | ✗ | std::ostringstream oss("wrong path id. "); | |
1793 | ✗ | oss << "Number path: " << problemSolver()->paths().size() << "."; | |
1794 | ✗ | throw std::runtime_error(oss.str()); | |
1795 | } | ||
1796 | ✗ | PathVectorPtr_t initPath = problemSolver()->paths()[pathId]; | |
1797 | ✗ | if ((start < initPath->timeRange().first) || | |
1798 | ✗ | end > initPath->timeRange().second) { | |
1799 | ✗ | std::ostringstream oss("Parameters out of bounds. "); | |
1800 | ✗ | oss << "For pathId = " << pathId << ", lenght = [" | |
1801 | ✗ | << initPath->timeRange().first << " ; " | |
1802 | ✗ | << initPath->timeRange().second << "]."; | |
1803 | ✗ | throw std::runtime_error(oss.str()); | |
1804 | } | ||
1805 | ✗ | core::PathPtr_t path = initPath->extract(core::interval_t(start, end)); | |
1806 | ✗ | PathVectorPtr_t pathVector = dynamic_pointer_cast<core::PathVector>(path); | |
1807 | ✗ | if (pathVector) | |
1808 | ✗ | problemSolver()->addPath(pathVector); | |
1809 | else { | ||
1810 | ✗ | std::ostringstream oss("Error during dynamic-cast in extractPath "); | |
1811 | ✗ | throw std::runtime_error(oss.str()); | |
1812 | } | ||
1813 | ✗ | } catch (const std::exception& exc) { | |
1814 | ✗ | throw hpp::Error(exc.what()); | |
1815 | } | ||
1816 | } | ||
1817 | |||
1818 | // --------------------------------------------------------------- | ||
1819 | |||
1820 | ✗ | void Problem::erasePath(ULong pathId) { | |
1821 | try { | ||
1822 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1823 | ✗ | std::ostringstream oss("wrong path id. "); | |
1824 | ✗ | oss << "Number path: " << problemSolver()->paths().size() << "."; | |
1825 | ✗ | throw std::runtime_error(oss.str()); | |
1826 | } | ||
1827 | ✗ | problemSolver()->erasePath(pathId); | |
1828 | ✗ | } catch (const std::exception& exc) { | |
1829 | ✗ | throw hpp::Error(exc.what()); | |
1830 | } | ||
1831 | } | ||
1832 | |||
1833 | // --------------------------------------------------------------- | ||
1834 | |||
1835 | ✗ | bool Problem::projectPath(ULong pathId) { | |
1836 | try { | ||
1837 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1838 | ✗ | std::ostringstream oss("wrong path id: "); | |
1839 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1840 | ✗ | << "."; | |
1841 | ✗ | throw std::runtime_error(oss.str()); | |
1842 | } | ||
1843 | ✗ | PathVectorPtr_t initial = problemSolver()->paths()[pathId]; | |
1844 | ✗ | core::PathProjectorPtr_t pp = problemSolver()->problem()->pathProjector(); | |
1845 | ✗ | if (!pp) throw Error("There is no path projector"); | |
1846 | |||
1847 | ✗ | PathPtr_t proj; | |
1848 | ✗ | bool success = pp->apply(initial, proj); | |
1849 | |||
1850 | PathVectorPtr_t path(core::PathVector::create( | ||
1851 | ✗ | initial->outputSize(), initial->outputDerivativeSize())); | |
1852 | ✗ | path->appendPath(proj); | |
1853 | ✗ | problemSolver()->addPath(path); | |
1854 | ✗ | return success; | |
1855 | ✗ | } catch (const std::exception& exc) { | |
1856 | ✗ | throw hpp::Error(exc.what()); | |
1857 | } | ||
1858 | } | ||
1859 | |||
1860 | // --------------------------------------------------------------- | ||
1861 | |||
1862 | ✗ | void Problem::interruptPathPlanning() { problemSolver()->interrupt(); } | |
1863 | |||
1864 | // --------------------------------------------------------------- | ||
1865 | |||
1866 | ✗ | Long Problem::numberPaths() { | |
1867 | try { | ||
1868 | ✗ | return (Long)problemSolver()->paths().size(); | |
1869 | ✗ | } catch (std::exception& exc) { | |
1870 | ✗ | throw hpp::Error(exc.what()); | |
1871 | } | ||
1872 | } | ||
1873 | |||
1874 | // --------------------------------------------------------------- | ||
1875 | |||
1876 | ✗ | hpp::intSeq* Problem::optimizePath(ULong pathId) { | |
1877 | try { | ||
1878 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1879 | ✗ | std::ostringstream oss("wrong path id: "); | |
1880 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1881 | ✗ | << "."; | |
1882 | ✗ | throw std::runtime_error(oss.str()); | |
1883 | } | ||
1884 | // Start timer | ||
1885 | boost::posix_time::ptime start = | ||
1886 | ✗ | boost::posix_time::microsec_clock::universal_time(); | |
1887 | |||
1888 | ✗ | PathVectorPtr_t initial = problemSolver()->paths()[pathId]; | |
1889 | ✗ | problemSolver()->optimizePath(initial); | |
1890 | |||
1891 | // Stop timer | ||
1892 | boost::posix_time::time_duration time = | ||
1893 | ✗ | boost::posix_time::microsec_clock::universal_time() - start; | |
1894 | |||
1895 | ✗ | hpp::intSeq* ret = new hpp::intSeq; | |
1896 | ✗ | ret->length(4); | |
1897 | ✗ | (*ret)[0] = static_cast<CORBA::Long>(time.hours()); | |
1898 | ✗ | (*ret)[1] = static_cast<CORBA::Long>(time.minutes()); | |
1899 | ✗ | (*ret)[2] = static_cast<CORBA::Long>(time.seconds()); | |
1900 | ✗ | (*ret)[3] = static_cast<CORBA::Long>(time.fractional_seconds() / 1000); | |
1901 | ✗ | return ret; | |
1902 | ✗ | } catch (const std::exception& exc) { | |
1903 | ✗ | throw hpp::Error(exc.what()); | |
1904 | } | ||
1905 | } | ||
1906 | |||
1907 | // --------------------------------------------------------------- | ||
1908 | |||
1909 | ✗ | Double Problem::pathLength(ULong pathId) { | |
1910 | try { | ||
1911 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1912 | ✗ | std::ostringstream oss("wrong path id: "); | |
1913 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1914 | ✗ | << "."; | |
1915 | ✗ | throw std::runtime_error(oss.str()); | |
1916 | } | ||
1917 | ✗ | return problemSolver()->paths()[pathId]->length(); | |
1918 | ✗ | } catch (std::exception& exc) { | |
1919 | ✗ | throw hpp::Error(exc.what()); | |
1920 | } | ||
1921 | } | ||
1922 | |||
1923 | // --------------------------------------------------------------- | ||
1924 | |||
1925 | ✗ | hpp::floatSeq* Problem::configAtParam(ULong pathId, Double atDistance) { | |
1926 | try { | ||
1927 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1928 | ✗ | std::ostringstream oss("wrong path id: "); | |
1929 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1930 | ✗ | << "."; | |
1931 | ✗ | throw std::runtime_error(oss.str()); | |
1932 | } | ||
1933 | ✗ | PathPtr_t path = problemSolver()->paths()[pathId]; | |
1934 | bool success; | ||
1935 | ✗ | Configuration_t config = path->eval(atDistance, success); | |
1936 | ✗ | if (!success) { | |
1937 | ✗ | throw std::runtime_error( | |
1938 | "Failed to apply constraint in path " | ||
1939 | ✗ | "evaluation."); | |
1940 | } | ||
1941 | // Allocate result now that the size is known. | ||
1942 | ✗ | std::size_t size = config.size(); | |
1943 | ✗ | double* dofArray = hpp::floatSeq::allocbuf((ULong)size); | |
1944 | hpp::floatSeq* floatSeq = new hpp::floatSeq( | ||
1945 | ✗ | (CORBA::ULong)size, (CORBA::ULong)size, dofArray, true); | |
1946 | ✗ | for (std::size_t i = 0; i < size; ++i) { | |
1947 | ✗ | dofArray[(CORBA::ULong)i] = config[i]; | |
1948 | } | ||
1949 | ✗ | return floatSeq; | |
1950 | ✗ | } catch (const std::exception& exc) { | |
1951 | ✗ | throw hpp::Error(exc.what()); | |
1952 | } | ||
1953 | } | ||
1954 | |||
1955 | // --------------------------------------------------------------- | ||
1956 | |||
1957 | ✗ | hpp::floatSeq* Problem::derivativeAtParam(ULong pathId, ULong order, | |
1958 | Double atDistance) { | ||
1959 | try { | ||
1960 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1961 | ✗ | std::ostringstream oss("wrong path id: "); | |
1962 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1963 | ✗ | << "."; | |
1964 | ✗ | throw std::runtime_error(oss.str()); | |
1965 | } | ||
1966 | ✗ | PathPtr_t path = problemSolver()->paths()[pathId]; | |
1967 | ✗ | vector_t velocity(problemSolver()->robot()->numberDof()); | |
1968 | ✗ | path->derivative(velocity, atDistance, order); | |
1969 | ✗ | return vectorToFloatSeq(velocity); | |
1970 | ✗ | } catch (const std::exception& exc) { | |
1971 | ✗ | throw hpp::Error(exc.what()); | |
1972 | } | ||
1973 | } | ||
1974 | |||
1975 | // -------------------------------------------------------------- | ||
1976 | |||
1977 | ✗ | hpp::floatSeqSeq* Problem::getWaypoints(ULong pathId, hpp::floatSeq_out times) { | |
1978 | try { | ||
1979 | ✗ | if (pathId >= problemSolver()->paths().size()) { | |
1980 | ✗ | std::ostringstream oss("wrong path id: "); | |
1981 | ✗ | oss << pathId << ", number path: " << problemSolver()->paths().size() | |
1982 | ✗ | << "."; | |
1983 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
1984 | } | ||
1985 | ✗ | PathVectorPtr_t path = problemSolver()->paths()[pathId]; | |
1986 | PathVectorPtr_t flat = core::PathVector::create( | ||
1987 | ✗ | path->outputSize(), path->outputDerivativeSize()); | |
1988 | ✗ | path->flatten(flat); | |
1989 | ✗ | core::matrix_t points(flat->numberPaths() + 1, path->outputSize()); | |
1990 | ✗ | core::vector_t ts(flat->numberPaths() + 1); | |
1991 | ✗ | ts(0) = flat->timeRange().first; | |
1992 | ✗ | for (std::size_t i = 0; i < flat->numberPaths(); ++i) { | |
1993 | ✗ | points.row(i) = flat->pathAtRank(i)->initial(); | |
1994 | ✗ | ts(i + 1) = ts(i) + flat->pathAtRank(i)->length(); | |
1995 | } | ||
1996 | ✗ | points.row(flat->numberPaths()) = flat->end(); | |
1997 | ✗ | times = vectorToFloatSeq(ts); | |
1998 | ✗ | return matrixToFloatSeqSeq(points); | |
1999 | ✗ | } catch (const std::exception& exc) { | |
2000 | ✗ | throw hpp::Error(exc.what()); | |
2001 | } | ||
2002 | } | ||
2003 | |||
2004 | // --------------------------------------------------------------- | ||
2005 | |||
2006 | ✗ | hpp::floatSeqSeq* Problem::nodes() { | |
2007 | hpp::floatSeqSeq* res; | ||
2008 | try { | ||
2009 | ✗ | const Nodes_t& nodes(problemSolver()->roadmap()->nodes()); | |
2010 | ✗ | res = new hpp::floatSeqSeq; | |
2011 | ✗ | res->length((CORBA::ULong)nodes.size()); | |
2012 | ✗ | std::size_t i = 0; | |
2013 | ✗ | for (Nodes_t::const_iterator itNode = nodes.begin(); itNode != nodes.end(); | |
2014 | ✗ | itNode++) { | |
2015 | ✗ | Configuration_t config = (*itNode)->configuration(); | |
2016 | ✗ | ULong size = (ULong)config.size(); | |
2017 | ✗ | double* dofArray = hpp::floatSeq::allocbuf(size); | |
2018 | ✗ | hpp::floatSeq floats(size, size, dofArray, true); | |
2019 | // convert the config in dofseq | ||
2020 | ✗ | for (size_type j = 0; j < config.size(); ++j) { | |
2021 | ✗ | dofArray[j] = (config)[j]; | |
2022 | } | ||
2023 | ✗ | (*res)[(CORBA::ULong)i] = floats; | |
2024 | ✗ | ++i; | |
2025 | } | ||
2026 | ✗ | } catch (const std::exception& exc) { | |
2027 | ✗ | throw hpp::Error(exc.what()); | |
2028 | } | ||
2029 | ✗ | return res; | |
2030 | } | ||
2031 | |||
2032 | // --------------------------------------------------------------- | ||
2033 | |||
2034 | ✗ | Long Problem::numberEdges() { | |
2035 | ✗ | return (Long)problemSolver()->roadmap()->edges().size(); | |
2036 | } | ||
2037 | |||
2038 | // --------------------------------------------------------------- | ||
2039 | |||
2040 | ✗ | void Problem::edge(ULong edgeId, hpp::floatSeq_out q1, hpp::floatSeq_out q2) { | |
2041 | try { | ||
2042 | ✗ | const Edges_t& edges(problemSolver()->roadmap()->edges()); | |
2043 | ✗ | Edges_t::const_iterator itEdge = edges.begin(); | |
2044 | ✗ | std::size_t i = 0; | |
2045 | ✗ | while (i < edgeId) { | |
2046 | ✗ | ++i; | |
2047 | ✗ | itEdge++; | |
2048 | } | ||
2049 | ✗ | Configuration_t config1 = (*itEdge)->from()->configuration(); | |
2050 | ✗ | Configuration_t config2 = (*itEdge)->to()->configuration(); | |
2051 | ✗ | ULong size = (ULong)config1.size(); | |
2052 | |||
2053 | ✗ | hpp::floatSeq* q1_ptr = new hpp::floatSeq(); | |
2054 | ✗ | q1_ptr->length(size); | |
2055 | ✗ | hpp::floatSeq* q2_ptr = new hpp::floatSeq(); | |
2056 | ✗ | q2_ptr->length(size); | |
2057 | |||
2058 | ✗ | for (i = 0; i < size; ++i) { | |
2059 | ✗ | (*q1_ptr)[(CORBA::ULong)i] = (config1)[i]; | |
2060 | ✗ | (*q2_ptr)[(CORBA::ULong)i] = (config2)[i]; | |
2061 | } | ||
2062 | ✗ | q1 = q1_ptr; | |
2063 | ✗ | q2 = q2_ptr; | |
2064 | ✗ | } catch (const std::exception& exc) { | |
2065 | ✗ | throw hpp::Error(exc.what()); | |
2066 | } | ||
2067 | } | ||
2068 | |||
2069 | // --------------------------------------------------------------- | ||
2070 | |||
2071 | ✗ | Long Problem::numberNodes() { | |
2072 | try { | ||
2073 | ✗ | if (problemSolver()->roadmap()) { | |
2074 | ✗ | return (Long)problemSolver()->roadmap()->nodes().size(); | |
2075 | } else { | ||
2076 | ✗ | throw std::runtime_error("No roadmap in problem solver."); | |
2077 | } | ||
2078 | ✗ | } catch (const std::exception& exc) { | |
2079 | ✗ | throw hpp::Error(exc.what()); | |
2080 | } | ||
2081 | } | ||
2082 | |||
2083 | // --------------------------------------------------------------- | ||
2084 | |||
2085 | ✗ | hpp::floatSeq* Problem::node(ULong nodeId) { | |
2086 | try { | ||
2087 | ✗ | const Nodes_t& nodes(problemSolver()->roadmap()->nodes()); | |
2088 | |||
2089 | ✗ | if (nodes.size() > nodeId) { | |
2090 | ✗ | Nodes_t::const_iterator itNode = std::next(nodes.begin(), nodeId); | |
2091 | ✗ | Configuration_t conf = (*itNode)->configuration(); | |
2092 | ✗ | ULong size = (ULong)conf.size(); | |
2093 | |||
2094 | ✗ | hpp::floatSeq* q_ptr = new hpp::floatSeq(); | |
2095 | ✗ | q_ptr->length(size); | |
2096 | |||
2097 | ✗ | for (ULong i = 0; i < size; ++i) { | |
2098 | ✗ | (*q_ptr)[i] = (conf)[i]; | |
2099 | } | ||
2100 | ✗ | return q_ptr; | |
2101 | ✗ | } else { | |
2102 | ✗ | std::ostringstream oss("wrong nodeId :"); | |
2103 | ✗ | oss << nodeId << ", number of nodes: " << nodes.size() << "."; | |
2104 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
2105 | } | ||
2106 | ✗ | } catch (const std::exception& exc) { | |
2107 | ✗ | throw hpp::Error(exc.what()); | |
2108 | } | ||
2109 | } | ||
2110 | |||
2111 | // ----------------------------------------------------------------- | ||
2112 | |||
2113 | ✗ | Long Problem::connectedComponentOfEdge(ULong edgeId) { | |
2114 | try { | ||
2115 | ✗ | const Edges_t& edges(problemSolver()->roadmap()->edges()); | |
2116 | ✗ | if (edges.size() > edgeId) { | |
2117 | ✗ | Edges_t::const_iterator itEdge = std::next(edges.begin(), edgeId); | |
2118 | |||
2119 | const core::ConnectedComponents_t& ccs = | ||
2120 | ✗ | problemSolver()->roadmap()->connectedComponents(); | |
2121 | ✗ | core::ConnectedComponents_t::const_iterator itcc = ccs.begin(); | |
2122 | ✗ | for (std::size_t i = 0; i < ccs.size(); ++i) { | |
2123 | ✗ | if (*itcc == (*itEdge)->from()->connectedComponent()) { | |
2124 | ✗ | return (CORBA::Long)i; | |
2125 | } | ||
2126 | ✗ | itcc++; | |
2127 | } | ||
2128 | } | ||
2129 | ✗ | } catch (const std::exception& exc) { | |
2130 | ✗ | throw hpp::Error(exc.what()); | |
2131 | } | ||
2132 | ✗ | throw hpp::Error("Connected component not found"); | |
2133 | } | ||
2134 | |||
2135 | // ----------------------------------------------------------------- | ||
2136 | |||
2137 | ✗ | Long Problem::connectedComponentOfNode(ULong nodeId) { | |
2138 | try { | ||
2139 | ✗ | const Nodes_t& nodes(problemSolver()->roadmap()->nodes()); | |
2140 | ✗ | if (nodes.size() > nodeId) { | |
2141 | ✗ | Nodes_t::const_iterator itNode = std::next(nodes.begin(), nodeId); | |
2142 | |||
2143 | const core::ConnectedComponents_t& ccs = | ||
2144 | ✗ | problemSolver()->roadmap()->connectedComponents(); | |
2145 | ✗ | core::ConnectedComponents_t::const_iterator itcc = ccs.begin(); | |
2146 | ✗ | for (std::size_t i = 0; i < ccs.size(); ++i) { | |
2147 | ✗ | if (*itcc == (*itNode)->connectedComponent()) { | |
2148 | ✗ | return (CORBA::Long)i; | |
2149 | } | ||
2150 | ✗ | itcc++; | |
2151 | } | ||
2152 | } | ||
2153 | ✗ | } catch (const std::exception& exc) { | |
2154 | ✗ | throw hpp::Error(exc.what()); | |
2155 | } | ||
2156 | ✗ | throw hpp::Error("Connected component not found"); | |
2157 | } | ||
2158 | |||
2159 | // ----------------------------------------------------------------- | ||
2160 | |||
2161 | ✗ | Long Problem::numberConnectedComponents() { | |
2162 | ✗ | return (Long)problemSolver()->roadmap()->connectedComponents().size(); | |
2163 | } | ||
2164 | |||
2165 | // --------------------------------------------------------------- | ||
2166 | |||
2167 | ✗ | hpp::floatSeqSeq* Problem::nodesConnectedComponent(ULong connectedComponentId) { | |
2168 | const ConnectedComponents_t& connectedComponents( | ||
2169 | ✗ | problemSolver()->roadmap()->connectedComponents()); | |
2170 | ✗ | if ((std::size_t)connectedComponentId >= connectedComponents.size()) { | |
2171 | ✗ | std::ostringstream oss; | |
2172 | ✗ | oss << "connectedComponentId=" << connectedComponentId | |
2173 | ✗ | << " out of range [0," << connectedComponents.size() - 1 << "]."; | |
2174 | ✗ | throw hpp::Error(oss.str().c_str()); | |
2175 | } | ||
2176 | hpp::floatSeqSeq* res; | ||
2177 | try { | ||
2178 | ✗ | ConnectedComponents_t::const_iterator itcc = connectedComponents.begin(); | |
2179 | ✗ | ULong i = 0; | |
2180 | ✗ | while (i != connectedComponentId) { | |
2181 | ✗ | ++i; | |
2182 | ✗ | itcc++; | |
2183 | } | ||
2184 | ✗ | const NodeVector_t& nodes((*itcc)->nodes()); | |
2185 | ✗ | res = new hpp::floatSeqSeq; | |
2186 | ✗ | res->length((CORBA::ULong)nodes.size()); | |
2187 | ✗ | i = 0; | |
2188 | ✗ | for (NodeVector_t::const_iterator itNode = nodes.begin(); | |
2189 | ✗ | itNode != nodes.end(); itNode++) { | |
2190 | ✗ | Configuration_t config = (*itNode)->configuration(); | |
2191 | ✗ | ULong size = (ULong)config.size(); | |
2192 | ✗ | double* dofArray = hpp::floatSeq::allocbuf(size); | |
2193 | ✗ | hpp::floatSeq floats(size, size, dofArray, true); | |
2194 | // convert the config in dofseq | ||
2195 | ✗ | for (size_type j = 0; j < config.size(); ++j) { | |
2196 | ✗ | dofArray[j] = (config)[j]; | |
2197 | } | ||
2198 | ✗ | (*res)[i] = floats; | |
2199 | ✗ | ++i; | |
2200 | } | ||
2201 | ✗ | } catch (const std::exception& exc) { | |
2202 | ✗ | throw hpp::Error(exc.what()); | |
2203 | } | ||
2204 | ✗ | return res; | |
2205 | } | ||
2206 | |||
2207 | // --------------------------------------------------------------- | ||
2208 | |||
2209 | ✗ | hpp::floatSeq* Problem::getNearestConfig(const hpp::floatSeq& config, | |
2210 | const Long connectedComponentId, | ||
2211 | hpp::core::value_type& distance) { | ||
2212 | hpp::floatSeq* res; | ||
2213 | try { | ||
2214 | const hpp::core::ConnectedComponents_t& connectedComponents( | ||
2215 | ✗ | problemSolver()->roadmap()->connectedComponents()); | |
2216 | hpp::core::NodePtr_t nearest; | ||
2217 | ✗ | DevicePtr_t robot = getRobotOrThrow(problemSolver()); | |
2218 | ✗ | Configuration_t configuration = floatSeqToConfig(robot, config, true); | |
2219 | ✗ | if (connectedComponentId < 0) { | |
2220 | nearest = | ||
2221 | ✗ | problemSolver()->roadmap()->nearestNode(configuration, distance); | |
2222 | } else { | ||
2223 | ✗ | if ((std::size_t)connectedComponentId >= connectedComponents.size()) { | |
2224 | ✗ | std::ostringstream oss; | |
2225 | ✗ | oss << "connectedComponentId=" << connectedComponentId | |
2226 | ✗ | << " out of range [0," << connectedComponents.size() - 1 << "]."; | |
2227 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
2228 | } | ||
2229 | hpp::core::ConnectedComponents_t::const_iterator itcc = | ||
2230 | ✗ | connectedComponents.begin(); | |
2231 | ✗ | std::advance(itcc, connectedComponentId); | |
2232 | ✗ | nearest = problemSolver()->roadmap()->nearestNode(configuration, *itcc, | |
2233 | distance); | ||
2234 | } | ||
2235 | ✗ | if (!nearest) throw hpp::Error("Nearest node not found"); | |
2236 | ✗ | res = vectorToFloatSeq(nearest->configuration()); | |
2237 | ✗ | } catch (const std::exception& exc) { | |
2238 | ✗ | throw hpp::Error(exc.what()); | |
2239 | } | ||
2240 | ✗ | return res; | |
2241 | } | ||
2242 | |||
2243 | // --------------------------------------------------------------- | ||
2244 | |||
2245 | ✗ | void Problem::clearRoadmap() { | |
2246 | try { | ||
2247 | ✗ | problemSolver()->roadmap()->clear(); | |
2248 | ✗ | } catch (const std::exception& exc) { | |
2249 | ✗ | throw hpp::Error(exc.what()); | |
2250 | } | ||
2251 | } | ||
2252 | |||
2253 | // --------------------------------------------------------------- | ||
2254 | |||
2255 | ✗ | void Problem::resetRoadmap() { | |
2256 | try { | ||
2257 | ✗ | problemSolver()->resetRoadmap(); | |
2258 | ✗ | } catch (const std::exception& exc) { | |
2259 | ✗ | throw hpp::Error(exc.what()); | |
2260 | } | ||
2261 | } | ||
2262 | |||
2263 | ✗ | void Problem::saveRoadmap(const char* filename) { | |
2264 | try { | ||
2265 | ✗ | core::ProblemSolverPtr_t ps(problemSolver()); | |
2266 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
2267 | |||
2268 | using namespace core::parser; | ||
2269 | |||
2270 | typedef hpp::serialization::archive_tpl< | ||
2271 | boost::archive::binary_oarchive, | ||
2272 | hpp::serialization::remove_duplicate::vector_archive> | ||
2273 | archive_type; | ||
2274 | ✗ | core::RoadmapPtr_t roadmap(ps->roadmap()); | |
2275 | ✗ | serializeRoadmap<archive_type>(roadmap, std::string(filename), | |
2276 | ✗ | make_nvp(robot->name(), robot.get())); | |
2277 | ✗ | } catch (const std::exception& exc) { | |
2278 | ✗ | throw hpp::Error(exc.what()); | |
2279 | } | ||
2280 | } | ||
2281 | |||
2282 | ✗ | void Problem::loadRoadmap(const char* filename) { | |
2283 | try { | ||
2284 | ✗ | core::ProblemSolverPtr_t ps(problemSolver()); | |
2285 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
2286 | |||
2287 | using namespace core::parser; | ||
2288 | |||
2289 | typedef hpp::serialization::archive_tpl< | ||
2290 | boost::archive::binary_iarchive, | ||
2291 | hpp::serialization::remove_duplicate::vector_archive> | ||
2292 | archive_type; | ||
2293 | ✗ | hpp::core::RoadmapPtr_t roadmap; | |
2294 | ✗ | serializeRoadmap<archive_type>(roadmap, std::string(filename), | |
2295 | ✗ | make_nvp(robot->name(), robot.get())); | |
2296 | ✗ | problemSolver()->roadmap(roadmap); | |
2297 | ✗ | } catch (const std::exception& exc) { | |
2298 | ✗ | throw hpp::Error(exc.what()); | |
2299 | } | ||
2300 | } | ||
2301 | |||
2302 | 1 | void Problem::savePath(hpp::core_idl::Path_ptr _path, const char* filename) { | |
2303 | typedef hpp::serialization::archive_tpl< | ||
2304 | boost::archive::binary_oarchive, | ||
2305 | hpp::serialization::remove_duplicate::vector_archive> | ||
2306 | archive_type; | ||
2307 | |||
2308 | core::PathPtr_t path = | ||
2309 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | reference_to_object<core::Path>(server_->parent(), _path); |
2310 | try { | ||
2311 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::ofstream fs(filename); |
2312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | archive_type ar(fs); |
2313 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ar.initialize(); |
2314 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ar << hpp::serialization::make_nvp("path", path); |
2315 |
0/2✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
1 | } catch (const std::exception& exc) { |
2316 | ✗ | throw hpp::Error(exc.what()); | |
2317 | } | ||
2318 | 1 | } | |
2319 | |||
2320 | 1 | hpp::core_idl::Path_ptr Problem::loadPath(const char* filename) { | |
2321 | typedef hpp::serialization::archive_tpl< | ||
2322 | boost::archive::binary_iarchive, | ||
2323 | hpp::serialization::remove_duplicate::vector_archive> | ||
2324 | archive_type; | ||
2325 | |||
2326 | try { | ||
2327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | core::ProblemSolverPtr_t ps(problemSolver()); |
2328 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | DevicePtr_t robot = getRobotOrThrow(ps); |
2329 | |||
2330 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::ifstream fs(filename); |
2331 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | archive_type ar(fs); |
2332 | // Skip rebuilding the robot. The name should the same as the robot name | ||
2333 | // used when serializing. | ||
2334 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | ar.insert(robot->name(), robot.get()); |
2335 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ar.initialize(); |
2336 | 1 | core::PathPtr_t path; | |
2337 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ar >> hpp::serialization::make_nvp("path", path); |
2338 | |||
2339 | hpp::core_idl::Path_var d = | ||
2340 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | makeServantDownCast<core_impl::Path>(server_->parent(), path); |
2341 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return d._retn(); |
2342 |
0/2✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
1 | } catch (const std::exception& exc) { |
2343 | ✗ | throw hpp::Error(exc.what()); | |
2344 | } | ||
2345 | } | ||
2346 | |||
2347 | // --------------------------------------------------------------- | ||
2348 | |||
2349 | ✗ | void Problem::scCreateScalarMultiply(const char* outName, Double scalar, | |
2350 | const char* inName) { | ||
2351 | ✗ | if (!problemSolver()->robot()) throw hpp::Error("No robot loaded"); | |
2352 | try { | ||
2353 | ✗ | std::string inN(inName); | |
2354 | ✗ | std::string outN(outName); | |
2355 | ✗ | if (!problemSolver()->numericalConstraints.has(inN)) | |
2356 | ✗ | throw Error(("Constraint " + inN + " not found").c_str()); | |
2357 | ✗ | ImplicitPtr_t in = problemSolver()->numericalConstraints.get(inN); | |
2358 | typedef constraints::FunctionExp<constraints::DifferentiableFunction> | ||
2359 | FunctionExp_t; | ||
2360 | typedef constraints::ScalarMultiply<FunctionExp_t> Expr_t; | ||
2361 | constraints::DifferentiableFunctionPtr_t out = | ||
2362 | ✗ | constraints::SymbolicFunction<Expr_t>::create( | |
2363 | ✗ | outN, problemSolver()->robot(), | |
2364 | ✗ | constraints::Traits<Expr_t>::Ptr_t( | |
2365 | ✗ | new Expr_t(scalar, FunctionExp_t::create(in->functionPtr())))); | |
2366 | |||
2367 | ✗ | problemSolver()->addNumericalConstraint( | |
2368 | ✗ | outN, Implicit::create(out, in->comparisonType())); | |
2369 | ✗ | } catch (const std::exception& exc) { | |
2370 | ✗ | throw hpp::Error(exc.what()); | |
2371 | } | ||
2372 | } | ||
2373 | |||
2374 | // --------------------------------------------------------------- | ||
2375 | |||
2376 | ✗ | hpp::core_idl::Distance_ptr Problem::getDistance() { | |
2377 | try { | ||
2378 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2379 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
2380 | ✗ | core::DistancePtr_t distance = problem(ps, true)->distance(); | |
2381 | |||
2382 | hpp::core_idl::Distance_var d = makeServantDownCast<core_impl::Distance>( | ||
2383 | ✗ | server_->parent(), core_impl::Distance::Storage(distance)); | |
2384 | //(server_->parent(), core_impl::Distance::Storage (robot, distance)); | ||
2385 | ✗ | return d._retn(); | |
2386 | ✗ | } catch (const std::exception& exc) { | |
2387 | ✗ | throw hpp::Error(exc.what()); | |
2388 | } | ||
2389 | } | ||
2390 | |||
2391 | // --------------------------------------------------------------- | ||
2392 | |||
2393 | ✗ | void Problem::setDistance(hpp::core_idl::Distance_ptr distance) { | |
2394 | ✗ | core::DistancePtr_t d; | |
2395 | try { | ||
2396 | ✗ | d = reference_to_object<core::Distance>(server_->parent(), distance); | |
2397 | ✗ | } catch (const Error& e) { | |
2398 | // TODO in this case, we should define a distance from the CORBA type. | ||
2399 | // This would allow to implement a distance class in Python. | ||
2400 | ✗ | throw; | |
2401 | } | ||
2402 | |||
2403 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2404 | ✗ | core::ProblemPtr_t p = problem(ps, true); | |
2405 | ✗ | p->distance(d); | |
2406 | } | ||
2407 | |||
2408 | // --------------------------------------------------------------- | ||
2409 | |||
2410 | 1 | hpp::core_idl::Path_ptr Problem::getPath(ULong pathId) { | |
2411 | try { | ||
2412 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | core::ProblemSolverPtr_t ps = problemSolver(); |
2413 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
|
1 | if (pathId >= ps->paths().size()) { |
2414 | ✗ | HPP_THROW(Error, "wrong path id: " << pathId << ", number path: " | |
2415 | << ps->paths().size() << "."); | ||
2416 | } | ||
2417 | |||
2418 | 1 | core::PathVectorPtr_t pv = ps->paths()[pathId]; | |
2419 | hpp::core_idl::Path_var d = | ||
2420 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | makeServantDownCast<core_impl::Path>(server_->parent(), pv); |
2421 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return d._retn(); |
2422 |
0/2✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
1 | } catch (const std::exception& exc) { |
2423 | ✗ | throw hpp::Error(exc.what()); | |
2424 | } | ||
2425 | } | ||
2426 | |||
2427 | // --------------------------------------------------------------- | ||
2428 | |||
2429 | ✗ | ULong Problem::addPath(hpp::core_idl::PathVector_ptr _path) { | |
2430 | ✗ | core::PathVectorPtr_t path; | |
2431 | try { | ||
2432 | ✗ | path = reference_to_object<core::PathVector>(server_->parent(), _path); | |
2433 | ✗ | } catch (const Error& e) { | |
2434 | // TODO in this case, we should define a distance from the CORBA type. | ||
2435 | // This would allow to implement a distance class in Python. | ||
2436 | ✗ | throw; | |
2437 | } | ||
2438 | ✗ | if (!path) throw Error("Could not convert the path into a PathVector"); | |
2439 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2440 | ✗ | ps->addPath(path); | |
2441 | ✗ | return (ULong)(ps->paths().size() - 1); | |
2442 | } | ||
2443 | |||
2444 | // --------------------------------------------------------------- | ||
2445 | |||
2446 | ✗ | hpp::core_idl::SteeringMethod_ptr Problem::getSteeringMethod() { | |
2447 | try { | ||
2448 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2449 | ✗ | DevicePtr_t robot = getRobotOrThrow(ps); | |
2450 | ✗ | core::SteeringMethodPtr_t sm = problem(ps, true)->steeringMethod(); | |
2451 | |||
2452 | hpp::core_idl::SteeringMethod_var d = | ||
2453 | makeServantDownCast<core_impl::SteeringMethod>( | ||
2454 | ✗ | server_->parent(), core_impl::SteeringMethod::Storage(sm)); | |
2455 | // core_idl::SteeringMethod::Storage (robot, sm)); | ||
2456 | ✗ | return d._retn(); | |
2457 | ✗ | } catch (const std::exception& exc) { | |
2458 | ✗ | throw hpp::Error(exc.what()); | |
2459 | } | ||
2460 | } | ||
2461 | |||
2462 | // --------------------------------------------------------------- | ||
2463 | |||
2464 | ✗ | hpp::core_idl::PathValidation_ptr Problem::getPathValidation() { | |
2465 | try { | ||
2466 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2467 | ✗ | core::PathValidationPtr_t pv = problem(ps, true)->pathValidation(); | |
2468 | |||
2469 | hpp::core_idl::PathValidation_var d = | ||
2470 | makeServantDownCast<core_impl::PathValidation>( | ||
2471 | ✗ | server_->parent(), core_impl::PathValidation::Storage(pv)); | |
2472 | ✗ | return d._retn(); | |
2473 | ✗ | } catch (const std::exception& exc) { | |
2474 | ✗ | throw hpp::Error(exc.what()); | |
2475 | } | ||
2476 | } | ||
2477 | |||
2478 | // --------------------------------------------------------------- | ||
2479 | |||
2480 | ✗ | hpp::core_idl::PathPlanner_ptr Problem::getPathPlanner() { | |
2481 | try { | ||
2482 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2483 | ✗ | core::PathPlannerPtr_t pv = ps->pathPlanner(); | |
2484 | |||
2485 | hpp::core_idl::PathPlanner_var d = | ||
2486 | makeServantDownCast<core_impl::PathPlanner>( | ||
2487 | ✗ | server_->parent(), core_impl::PathPlanner::Storage(pv)); | |
2488 | ✗ | return d._retn(); | |
2489 | ✗ | } catch (const std::exception& exc) { | |
2490 | ✗ | throw hpp::Error(exc.what()); | |
2491 | } | ||
2492 | } | ||
2493 | |||
2494 | // --------------------------------------------------------------- | ||
2495 | |||
2496 | 3 | hpp::core_idl::Problem_ptr Problem::getProblem() { | |
2497 | try { | ||
2498 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | core::ProblemSolverPtr_t ps = problemSolver(); |
2499 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | core::ProblemPtr_t pb = problem(ps, true); |
2500 | |||
2501 | hpp::core_idl::Problem_var pb_idl = makeServantDownCast<core_impl::Problem>( | ||
2502 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | server_->parent(), core_impl::Problem::Storage(pb)); |
2503 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | return pb_idl._retn(); |
2504 |
0/2✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
3 | } catch (const std::exception& exc) { |
2505 | ✗ | throw hpp::Error(exc.what()); | |
2506 | } | ||
2507 | } | ||
2508 | |||
2509 | // --------------------------------------------------------------- | ||
2510 | |||
2511 | ✗ | hpp::constraints_idl::Implicit_ptr Problem::getConstraint(const char* name) { | |
2512 | try { | ||
2513 | ✗ | const std::string fn(name); | |
2514 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2515 | ✗ | if (!ps->numericalConstraints.has(fn)) | |
2516 | ✗ | throw Error(("Constraint " + fn + " not found").c_str()); | |
2517 | |||
2518 | hpp::constraints_idl::Implicit_var d = | ||
2519 | makeServantDownCast<constraints_impl::Implicit>( | ||
2520 | ✗ | server_->parent(), ps->numericalConstraints.get(fn)); | |
2521 | ✗ | return d._retn(); | |
2522 | ✗ | } catch (const std::exception& exc) { | |
2523 | ✗ | throw hpp::Error(exc.what()); | |
2524 | } | ||
2525 | } | ||
2526 | |||
2527 | // --------------------------------------------------------------- | ||
2528 | |||
2529 | ✗ | void Problem::setRobot(hpp::pinocchio_idl::Device_ptr _robot) { | |
2530 | ✗ | pinocchio::DevicePtr_t robot; | |
2531 | try { | ||
2532 | ✗ | robot = reference_to_object<pinocchio::Device>(server_->parent(), _robot); | |
2533 | ✗ | } catch (const Error& e) { | |
2534 | ✗ | throw; | |
2535 | } | ||
2536 | ✗ | if (!robot) throw Error("Could not get the robot"); | |
2537 | ✗ | problemSolver()->robot(robot); | |
2538 | } | ||
2539 | |||
2540 | ✗ | pinocchio_idl::CollisionObject_ptr Problem::getObstacle(const char* name) { | |
2541 | try { | ||
2542 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2543 | pinocchio_idl::CollisionObject_var o = | ||
2544 | makeServantDownCast<pinocchio_impl::CollisionObject>( | ||
2545 | ✗ | server_->parent(), ps->obstacle(name)); | |
2546 | ✗ | return o._retn(); | |
2547 | ✗ | } catch (const std::exception& exc) { | |
2548 | ✗ | throw hpp::Error(exc.what()); | |
2549 | } | ||
2550 | } | ||
2551 | |||
2552 | ✗ | core_idl::Problem_ptr Problem::createProblem(pinocchio_idl::Device_ptr robot) { | |
2553 | try { | ||
2554 | core_idl::Problem_var o = makeServantDownCast<core_impl::Problem>( | ||
2555 | ✗ | server_->parent(), | |
2556 | ✗ | core::Problem::create( | |
2557 | ✗ | reference_to_object<pinocchio::Device>(server_->parent(), robot))); | |
2558 | ✗ | return o._retn(); | |
2559 | ✗ | } catch (const std::exception& exc) { | |
2560 | ✗ | throw hpp::Error(exc.what()); | |
2561 | } | ||
2562 | } | ||
2563 | |||
2564 | ✗ | core_idl::Roadmap_ptr Problem::createRoadmap(core_idl::Distance_ptr distance, | |
2565 | pinocchio_idl::Device_ptr robot) { | ||
2566 | try { | ||
2567 | core_idl::Roadmap_var o = makeServantDownCast<core_impl::Roadmap>( | ||
2568 | ✗ | server_->parent(), | |
2569 | ✗ | core::Roadmap::create( | |
2570 | ✗ | reference_to_object<core::Distance>(server_->parent(), distance), | |
2571 | ✗ | reference_to_object<pinocchio::Device>(server_->parent(), robot))); | |
2572 | ✗ | return o._retn(); | |
2573 | ✗ | } catch (const std::exception& exc) { | |
2574 | ✗ | throw hpp::Error(exc.what()); | |
2575 | } | ||
2576 | } | ||
2577 | |||
2578 | ✗ | core_idl::Roadmap_ptr Problem::readRoadmap(const char* filename, | |
2579 | pinocchio_idl::Device_ptr robot) { | ||
2580 | try { | ||
2581 | DevicePtr_t device = | ||
2582 | ✗ | reference_to_object<pinocchio::Device>(server_->parent(), robot); | |
2583 | |||
2584 | ✗ | hpp::core::RoadmapPtr_t roadmap; | |
2585 | ✗ | std::string fn(filename); | |
2586 | ✗ | bool xml = (fn.size() >= 4 && fn.compare(fn.size() - 4, 4, ".xml") == 0); | |
2587 | using namespace core::parser; | ||
2588 | ✗ | if (xml) { | |
2589 | typedef hpp::serialization::archive_tpl< | ||
2590 | boost::archive::xml_iarchive, | ||
2591 | hpp::serialization::remove_duplicate::vector_archive> | ||
2592 | archive_type; | ||
2593 | ✗ | serializeRoadmap<archive_type>(roadmap, fn, | |
2594 | ✗ | make_nvp(device->name(), device.get())); | |
2595 | } else { | ||
2596 | typedef hpp::serialization::archive_tpl< | ||
2597 | boost::archive::binary_iarchive, | ||
2598 | hpp::serialization::remove_duplicate::vector_archive> | ||
2599 | archive_type; | ||
2600 | ✗ | serializeRoadmap<archive_type>(roadmap, fn, | |
2601 | ✗ | make_nvp(device->name(), device.get())); | |
2602 | } | ||
2603 | |||
2604 | core_idl::Roadmap_var o = | ||
2605 | ✗ | makeServantDownCast<core_impl::Roadmap>(server_->parent(), roadmap); | |
2606 | ✗ | return o._retn(); | |
2607 | ✗ | } catch (const std::exception& exc) { | |
2608 | ✗ | throw hpp::Error(exc.what()); | |
2609 | } | ||
2610 | } | ||
2611 | |||
2612 | ✗ | void Problem::writeRoadmap(const char* filename, core_idl::Roadmap_ptr _roadmap, | |
2613 | pinocchio_idl::Device_ptr robot) { | ||
2614 | try { | ||
2615 | DevicePtr_t device = | ||
2616 | ✗ | reference_to_object<pinocchio::Device>(server_->parent(), robot); | |
2617 | core::RoadmapPtr_t roadmap = | ||
2618 | ✗ | reference_to_object<core::Roadmap>(server_->parent(), _roadmap); | |
2619 | |||
2620 | ✗ | std::string fn(filename); | |
2621 | ✗ | bool xml = (fn.size() >= 4 && fn.compare(fn.size() - 4, 4, ".xml") == 0); | |
2622 | using namespace core::parser; | ||
2623 | ✗ | if (xml) { | |
2624 | typedef hpp::serialization::archive_tpl< | ||
2625 | boost::archive::xml_oarchive, | ||
2626 | hpp::serialization::remove_duplicate::vector_archive> | ||
2627 | archive_type; | ||
2628 | ✗ | serializeRoadmap<archive_type>(roadmap, fn, | |
2629 | ✗ | make_nvp(device->name(), device.get())); | |
2630 | } else { | ||
2631 | typedef hpp::serialization::archive_tpl< | ||
2632 | boost::archive::binary_oarchive, | ||
2633 | hpp::serialization::remove_duplicate::vector_archive> | ||
2634 | archive_type; | ||
2635 | ✗ | serializeRoadmap<archive_type>(roadmap, fn, | |
2636 | ✗ | make_nvp(device->name(), device.get())); | |
2637 | } | ||
2638 | ✗ | } catch (const std::exception& exc) { | |
2639 | ✗ | throw hpp::Error(exc.what()); | |
2640 | } | ||
2641 | } | ||
2642 | |||
2643 | ✗ | core_idl::PathPlanner_ptr Problem::createPathPlanner( | |
2644 | const char* type, core_idl::Problem_ptr problem, | ||
2645 | core_idl::Roadmap_ptr roadmap) { | ||
2646 | try { | ||
2647 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2648 | core_idl::PathPlanner_var o = makeServantDownCast<core_impl::PathPlanner>( | ||
2649 | ✗ | server_->parent(), | |
2650 | ✗ | (ps->pathPlanners.get(type))( | |
2651 | ✗ | reference_to_object<core::Problem>(server_->parent(), problem), | |
2652 | ✗ | reference_to_object<core::Roadmap>(server_->parent(), roadmap))); | |
2653 | ✗ | return o._retn(); | |
2654 | ✗ | } catch (const std::exception& exc) { | |
2655 | ✗ | throw hpp::Error(exc.what()); | |
2656 | } | ||
2657 | } | ||
2658 | ✗ | core_idl::PathOptimizer_ptr Problem::createPathOptimizer( | |
2659 | const char* type, core_idl::Problem_ptr problem) { | ||
2660 | try { | ||
2661 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2662 | core_idl::PathOptimizer_var o = | ||
2663 | makeServantDownCast<core_impl::PathOptimizer>( | ||
2664 | ✗ | server_->parent(), | |
2665 | ✗ | (ps->pathOptimizers.get(type))(reference_to_object<core::Problem>( | |
2666 | ✗ | server_->parent(), problem))); | |
2667 | ✗ | return o._retn(); | |
2668 | ✗ | } catch (const std::exception& exc) { | |
2669 | ✗ | throw hpp::Error(exc.what()); | |
2670 | } | ||
2671 | } | ||
2672 | |||
2673 | ✗ | core_idl::PathProjector_ptr Problem::createPathProjector( | |
2674 | const char* type, core_idl::Problem_ptr problem, value_type parameter) { | ||
2675 | try { | ||
2676 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2677 | core_idl::PathProjector_var o = | ||
2678 | makeServantDownCast<core_impl::PathProjector>( | ||
2679 | ✗ | server_->parent(), | |
2680 | ✗ | (ps->pathProjectors.get(type))( | |
2681 | ✗ | reference_to_object<core::Problem>(server_->parent(), problem), | |
2682 | ✗ | parameter)); | |
2683 | ✗ | return o._retn(); | |
2684 | ✗ | } catch (const std::exception& exc) { | |
2685 | ✗ | throw hpp::Error(exc.what()); | |
2686 | } | ||
2687 | } | ||
2688 | |||
2689 | ✗ | core_idl::PathValidation_ptr Problem::createPathValidation( | |
2690 | const char* type, pinocchio_idl::Device_ptr robot, value_type parameter) { | ||
2691 | try { | ||
2692 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2693 | core_idl::PathValidation_var o = | ||
2694 | makeServantDownCast<core_impl::PathValidation>( | ||
2695 | ✗ | server_->parent(), (ps->pathValidations.get(type))( | |
2696 | ✗ | reference_to_object<pinocchio::Device>( | |
2697 | ✗ | server_->parent(), robot), | |
2698 | ✗ | parameter)); | |
2699 | ✗ | return o._retn(); | |
2700 | ✗ | } catch (const std::exception& exc) { | |
2701 | ✗ | throw hpp::Error(exc.what()); | |
2702 | } | ||
2703 | } | ||
2704 | |||
2705 | ✗ | core_idl::ConfigurationShooter_ptr Problem::createConfigurationShooter( | |
2706 | const char* type, core_idl::Problem_ptr problem) { | ||
2707 | try { | ||
2708 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2709 | core_idl::ConfigurationShooter_var o = | ||
2710 | makeServantDownCast<core_impl::ConfigurationShooter>( | ||
2711 | ✗ | server_->parent(), (ps->configurationShooters.get(type))( | |
2712 | ✗ | reference_to_object<core::Problem>( | |
2713 | ✗ | server_->parent(), problem))); | |
2714 | ✗ | return o._retn(); | |
2715 | ✗ | } catch (const std::exception& exc) { | |
2716 | ✗ | throw hpp::Error(exc.what()); | |
2717 | } | ||
2718 | } | ||
2719 | ✗ | core_idl::ConfigValidation_ptr Problem::createConfigValidation( | |
2720 | const char* type, pinocchio_idl::Device_ptr robot) { | ||
2721 | try { | ||
2722 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2723 | core_idl::ConfigValidation_var o = | ||
2724 | makeServantDownCast<core_impl::ConfigValidation>( | ||
2725 | ✗ | server_->parent(), (ps->configValidations.get(type))( | |
2726 | ✗ | reference_to_object<pinocchio::Device>( | |
2727 | ✗ | server_->parent(), robot))); | |
2728 | ✗ | return o._retn(); | |
2729 | ✗ | } catch (const std::exception& exc) { | |
2730 | ✗ | throw hpp::Error(exc.what()); | |
2731 | } | ||
2732 | } | ||
2733 | ✗ | core_idl::Distance_ptr Problem::createDistance(const char* type, | |
2734 | core_idl::Problem_ptr problem) { | ||
2735 | try { | ||
2736 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2737 | core_idl::Distance_var o = makeServantDownCast<core_impl::Distance>( | ||
2738 | ✗ | server_->parent(), | |
2739 | ✗ | (ps->distances.get(type))( | |
2740 | ✗ | reference_to_object<core::Problem>(server_->parent(), problem))); | |
2741 | ✗ | return o._retn(); | |
2742 | ✗ | } catch (const std::exception& exc) { | |
2743 | ✗ | throw hpp::Error(exc.what()); | |
2744 | } | ||
2745 | } | ||
2746 | ✗ | core_idl::SteeringMethod_ptr Problem::createSteeringMethod( | |
2747 | const char* type, core_idl::Problem_ptr problem) { | ||
2748 | try { | ||
2749 | ✗ | core::ProblemSolverPtr_t ps = problemSolver(); | |
2750 | core_idl::SteeringMethod_var o = | ||
2751 | makeServantDownCast<core_impl::SteeringMethod>( | ||
2752 | ✗ | server_->parent(), | |
2753 | ✗ | (ps->steeringMethods.get(type))(reference_to_object<core::Problem>( | |
2754 | ✗ | server_->parent(), problem))); | |
2755 | ✗ | return o._retn(); | |
2756 | ✗ | } catch (const std::exception& exc) { | |
2757 | ✗ | throw hpp::Error(exc.what()); | |
2758 | } | ||
2759 | } | ||
2760 | 1 | core_idl::Constraint_ptr Problem::createConstraintSet( | |
2761 | pinocchio_idl::Device_ptr robot, const char* name) { | ||
2762 | try { | ||
2763 | pinocchio::DevicePtr_t device = | ||
2764 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | reference_to_object<pinocchio::Device>(server_->parent(), robot); |
2765 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | core::ConstraintSetPtr_t c = core::ConstraintSet::create(device, name); |
2766 | core_idl::Constraint_var o = | ||
2767 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | makeServantDownCast<core_impl::Constraint>(server_->parent(), c); |
2768 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return o._retn(); |
2769 |
0/2✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
1 | } catch (const std::exception& exc) { |
2770 | ✗ | throw hpp::Error(exc.what()); | |
2771 | } | ||
2772 | } | ||
2773 | 1 | core_idl::Constraint_ptr Problem::createConfigProjector( | |
2774 | pinocchio_idl::Device_ptr robot, const char* name, Double threshold, | ||
2775 | ULong iterations) { | ||
2776 | try { | ||
2777 | pinocchio::DevicePtr_t device = | ||
2778 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | reference_to_object<pinocchio::Device>(server_->parent(), robot); |
2779 | core::ConfigProjectorPtr_t c = | ||
2780 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | core::ConfigProjector::create(device, name, threshold, iterations); |
2781 | core_idl::Constraint_var o = | ||
2782 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | makeServantDownCast<core_impl::Constraint>(server_->parent(), c); |
2783 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return o._retn(); |
2784 |
0/2✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
1 | } catch (const std::exception& exc) { |
2785 | ✗ | throw hpp::Error(exc.what()); | |
2786 | } | ||
2787 | } | ||
2788 | |||
2789 | } // namespace impl | ||
2790 | } // namespace corbaServer | ||
2791 | } // namespace hpp | ||
2792 |