GCC Code Coverage Report

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