GCC Code Coverage Report

Directory: ./
File: src/problem.impl.cc
Date: 2025-05-11 11:10:19
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