problem-solver.cc 37.8 KB
Newer Older
1

2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

Joseph Mirabel's avatar
Joseph Mirabel committed
20
21
#include <hpp/core/problem-solver.hh>

Joseph Mirabel's avatar
Joseph Mirabel committed
22
23
#include <boost/bind.hpp>

24
25
#include <hpp/fcl/collision_utility.h>

Joseph Mirabel's avatar
Joseph Mirabel committed
26
#include <pinocchio/multibody/fcl.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
27
#include <pinocchio/multibody/geometry.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
28

29
#include <hpp/util/debug.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
30
#include <hpp/util/exception-factory.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
31

32
#include <hpp/pinocchio/collision-object.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
33

34
#include <hpp/constraints/differentiable-function.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
35

36
#include <hpp/core/configuration-shooter/uniform.hh>
Steve Tonneau's avatar
Steve Tonneau committed
37
#include <hpp/core/bi-rrt-planner.hh>
38
#include <hpp/core/configuration-shooter/gaussian.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
39
#include <hpp/core/config-projector.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
40
#include <hpp/core/constraint-set.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
41
42
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
#include <hpp/core/continuous-collision-checking/progressive.hh>
43
#include <hpp/core/diffusing-planner.hh>
44
#include <hpp/core/distance/reeds-shepp.hh>
45
#include <hpp/core/distance-between-objects.hh>
46
#include <hpp/core/discretized-collision-checking.hh>
47
#include <hpp/core/locked-joint.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
48
#include <hpp/core/numerical-constraint.hh>
Florent Lamiraux's avatar
Florent Lamiraux committed
49
#include <hpp/core/path-planner/k-prm-star.hh>
50
#include <hpp/core/path-projector/global.hh>
51
52
#include <hpp/core/path-projector/dichotomy.hh>
#include <hpp/core/path-projector/progressive.hh>
53
#include <hpp/core/path-projector/recursive-hermite.hh>
54
#include <hpp/core/path-optimization/gradient-based.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
55
#include <hpp/core/path-optimization/spline-gradient-based.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
56
#include <hpp/core/path-optimization/partial-shortcut.hh>
57
#include <hpp/core/path-optimization/config-optimization.hh>
58
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
59
#include <hpp/core/path-validation-report.hh>
60
// #include <hpp/core/problem-target/task-target.hh>
61
#include <hpp/core/problem-target/goal-configurations.hh>
62
#include <hpp/core/random-shortcut.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
63
#include <hpp/core/roadmap.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
64
#include <hpp/core/steering-method/dubins.hh>
65
#include <hpp/core/steering-method/hermite.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
66
67
68
#include <hpp/core/steering-method/reeds-shepp.hh>
#include <hpp/core/steering-method/snibud.hh>
#include <hpp/core/steering-method/straight.hh>
69
#include <hpp/core/visibility-prm-planner.hh>
70
#include <hpp/core/weighed-distance.hh>
71
72
#include <hpp/core/collision-validation.hh>
#include <hpp/core/joint-bound-validation.hh>
73
74
75

namespace hpp {
  namespace core {
76
    using boost::bind;
77
78
    using pinocchio::GeomIndex;

79
    using pinocchio::Model;
Joseph Mirabel's avatar
Joseph Mirabel committed
80
81
82
83
84
85
    using pinocchio::GeomModel;
    using pinocchio::GeomModelPtr_t;
    using pinocchio::GeomData;
    using pinocchio::GeomDataPtr_t;
    using pinocchio::CollisionObject;

86
87
88
89
90
91
92
    namespace {
      Model initObstacleModel () {
        Model model;
        model.addFrame(se3::Frame("obstacle_frame", 0, 0, Transform3f::Identity(), se3::BODY));
        return model;
      }

93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
      template<typename Container> void remove(Container& vector, std::size_t pos)
      {
        assert (pos < vector.size());
        typename Container::iterator it = vector.begin();
        std::advance(it, pos);
        vector.erase(it);
      }

      struct FindCollisionObject {
        FindCollisionObject (const GeomIndex& i) : geomIdx_ (i) {}
        bool operator() (const CollisionObjectPtr_t co) const {
          return co->indexInModel() == geomIdx_;
        }
        GeomIndex geomIdx_;
      };

      void remove(ObjectStdVector_t& vector, const GeomIndex& i)
      {
        ObjectStdVector_t::iterator it =
          std::find_if(vector.begin(), vector.end(), FindCollisionObject(i));
        if (it != vector.end()) vector.erase(it);
      }

116
117
118
      const Model obsModel = initObstacleModel();
    }

119
120
121
    // Struct that constructs an empty shared pointer to PathProjector.
    struct NonePathProjector
    {
122
      static PathProjectorPtr_t create (const Problem&,
123
					const value_type&)
124
125
126
      {
	return PathProjectorPtr_t ();
      }
127
    }; // struct NonePathProjector
128

129
    template <typename Derived> struct Factory {
130
      static boost::shared_ptr<Derived> create (const Problem& problem) { return Derived::create (problem); }
131
132
    };
    template <typename Derived> struct FactoryPP {
133
      static boost::shared_ptr<Derived> create (const Problem& problem, const value_type& value) { return Derived::create (problem, value); }
134
135
    };

136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
    template <typename T>
    boost::shared_ptr<T> createFromRobot (const Problem& p) { return T::create(p.robot()); }

    configurationShooter::GaussianPtr_t createGaussianConfigShooter (const Problem& p)
    {
      configurationShooter::GaussianPtr_t ptr = configurationShooter::Gaussian::create(p.robot());
      static const std::string useVel = "ConfigurationShooter/Gaussian/useRobotVelocity";
      static const std::string stdDev = "ConfigurationShooter/Gaussian/standardDeviation";
      if (p.getParameter(useVel).boolValue())
        ptr->sigmas (p.robot()->currentVelocity());
      else if (p.parameters.has(stdDev))
        ptr->sigma (p.getParameter (stdDev).floatValue());
      return ptr;
    }

151
152
153
154
155
156
157
158
159
160
161
162
    ProblemSolverPtr_t ProblemSolver::latest_ = 0x0;
    ProblemSolverPtr_t ProblemSolver::create ()
    {
      latest_ = new ProblemSolver ();
      return latest_;
    }

    ProblemSolverPtr_t ProblemSolver::latest ()
    {
      return latest_;
    }

163
    ProblemSolver::ProblemSolver () :
164
      constraints_ (), robot_ (), problem_ (NULL), pathPlanner_ (),
165
166
      roadmap_ (), paths_ (),
      pathProjectorType_ ("None"), pathProjectorTolerance_ (0.2),
167
      pathPlannerType_ ("DiffusingPlanner"),
168
      target_ (problemTarget::GoalConfigurations::create(NULL)),
169
      initConf_ (), goalConfigurations_ (),
170
      robotType_ ("hpp::pinocchio::Device"),
171
      configurationShooterType_ ("Uniform"),
172
      distanceType_("WeighedDistance"),
173
      steeringMethodType_ ("Straight"),
174
      pathOptimizerTypes_ (), pathOptimizers_ (),
175
      pathValidationType_ ("Discretized"), pathValidationTolerance_ (0.05),
176
      configValidationTypes_ (),
Joseph Mirabel's avatar
Joseph Mirabel committed
177
      collisionObstacles_ (), distanceObstacles_ (),
178
179
      obstacleModel_ (new GeomModel()), obstacleData_
      (new GeomData(*obstacleModel_)),
180
181
      errorThreshold_ (1e-4), maxIterProjection_ (20),
      maxIterPathPlanning_ (std::numeric_limits
182
			    <unsigned long int>::max ()),
183
      passiveDofsMap_ (), comcMap_ (),
184
      distanceBetweenObjects_ ()
185
    {
186
187
188
189
      robots.add (robotType_, Device_t::create);
      pathPlanners.add ("DiffusingPlanner",     DiffusingPlanner::createWithRoadmap);
      pathPlanners.add ("VisibilityPrmPlanner", VisibilityPrmPlanner::createWithRoadmap);
      pathPlanners.add ("BiRRTPlanner", BiRRTPlanner::createWithRoadmap);
Florent Lamiraux's avatar
Florent Lamiraux committed
190
      pathPlanners.add ("kPRM*", pathPlanner::kPrmStar::createWithRoadmap);
Joseph Mirabel's avatar
Joseph Mirabel committed
191

192
193
      configurationShooters.add ("Uniform" , createFromRobot<configurationShooter::Uniform>);
      configurationShooters.add ("Gaussian", createGaussianConfigShooter);
194
195

      distances.add ("Weighed",         WeighedDistance::createFromProblem);
196
      distances.add ("ReedsShepp",      bind (distance::ReedsShepp::create, _1));
Joseph Mirabel's avatar
Joseph Mirabel committed
197

198
      steeringMethods.add ("Straight",   Factory<steeringMethod::Straight>::create);
199
200
201
202
      steeringMethods.add ("ReedsShepp", steeringMethod::ReedsShepp::createWithGuess);
      steeringMethods.add ("Dubins",     steeringMethod::Dubins::createWithGuess);
      steeringMethods.add ("Snibud",     steeringMethod::Snibud::createWithGuess);
      steeringMethods.add ("Hermite",    steeringMethod::Hermite::create);
Joseph Mirabel's avatar
Joseph Mirabel committed
203

Joseph Mirabel's avatar
Joseph Mirabel committed
204
      // Store path optimization methods in map.
205
206
207
208
209
210
211
212
213
214
215
216
      pathOptimizers.add ("RandomShortcut",     RandomShortcut::create);
      pathOptimizers.add ("GradientBased",      pathOptimization::GradientBased::create);
      pathOptimizers.add ("PartialShortcut",    pathOptimization::PartialShortcut::create);
      pathOptimizers.add ("ConfigOptimization", pathOptimization::ConfigOptimization::create);
      pathOptimizers.add ("SimpleTimeParameterization", pathOptimization::SimpleTimeParameterization::create);

      // pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 1>::create);
      // pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 2>::create);
      // pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 3>::create);
      pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<path::BernsteinBasis, 1>::create);
      // pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<path::BernsteinBasis, 2>::create);
      pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<path::BernsteinBasis, 3>::create);
Joseph Mirabel's avatar
Joseph Mirabel committed
217

218
      // Store path validation methods in map.
219
220
221
      pathValidations.add ("Discretized", DiscretizedCollisionChecking::create);
      pathValidations.add ("Progressive", continuousCollisionChecking::Progressive::create);
      pathValidations.add ("Dichotomy",   continuousCollisionChecking::Dichotomy::create);
Joseph Mirabel's avatar
Joseph Mirabel committed
222

223
224
225
226
227
228
229
230
      // Store config validation methods in map.
      configValidations.add ("CollisionValidation", CollisionValidation::create);
      configValidations.add ("JointBoundValidation", JointBoundValidation::create);

      // Set default config validation methods.
      configValidationTypes_.push_back("CollisionValidation");
      configValidationTypes_.push_back("JointBoundValidation");

231
      // Store path projector methods in map.
232
233
234
235
236
      pathProjectors.add ("None",             NonePathProjector::create);
      pathProjectors.add ("Progressive",      FactoryPP<pathProjector::Progressive>::create);
      pathProjectors.add ("Dichotomy",        FactoryPP<pathProjector::Dichotomy>::create);
      pathProjectors.add ("Global",           FactoryPP<pathProjector::Global>::create);
      pathProjectors.add ("RecursiveHermite", FactoryPP<pathProjector::RecursiveHermite>::create);
237
238
239
240
241
242
243
    }

    ProblemSolver::~ProblemSolver ()
    {
      if (problem_) delete problem_;
    }

244
245
    void ProblemSolver::distanceType (const std::string& type)
    {
246
      if (!distances.has (type)) {
247
248
249
250
251
252
    throw std::runtime_error (std::string ("No distance method with name ") +
                  type);
      }
      distanceType_ = type;
    }

253
254
    void ProblemSolver::steeringMethodType (const std::string& type)
    {
255
      if (!steeringMethods.has (type)) {
256
257
258
259
	throw std::runtime_error (std::string ("No steering method with name ") +
				  type);
      }
      steeringMethodType_ = type;
260
      if (problem_) initSteeringMethod();
261
262
    }

263
264
    void ProblemSolver::pathPlannerType (const std::string& type)
    {
265
      if (!pathPlanners.has (type)) {
266
267
268
	throw std::runtime_error (std::string ("No path planner with name ") +
				  type);
      }
269
270
271
      pathPlannerType_ = type;
    }

272
273
    void ProblemSolver::configurationShooterType (const std::string& type)
    {
274
      if (!configurationShooters.has (type)) {
275
276
277
278
    throw std::runtime_error (std::string ("No configuration shooter with name ") +
                  type);
      }
      configurationShooterType_ = type;
279
280
      if (robot_ && problem_) {
        problem_->configurationShooter
281
          (configurationShooters.get (configurationShooterType_) (*problem_));
282
      }
283
284
    }

285
    void ProblemSolver::addPathOptimizer (const std::string& type)
286
    {
287
      if (!pathOptimizers.has (type)) {
288
289
290
	throw std::runtime_error (std::string ("No path optimizer with name ") +
				  type);
      }
291
292
293
294
295
296
      pathOptimizerTypes_.push_back (type);
    }

    void ProblemSolver::clearPathOptimizers ()
    {
      pathOptimizerTypes_.clear ();
297
      pathOptimizers_.clear ();
298
299
300
301
302
303
304
305
306
307
    }

    void ProblemSolver::optimizePath (PathVectorPtr_t path)
    {
      createPathOptimizers ();
      for (PathOptimizers_t::const_iterator it = pathOptimizers_.begin ();
	   it != pathOptimizers_.end (); ++it) {
	path = (*it)->optimize (path);
	paths_.push_back (path);
      }
308
309
    }

310
311
312
    void ProblemSolver::pathValidationType (const std::string& type,
					    const value_type& tolerance)
    {
313
      if (!pathValidations.has (type)) {
314
315
316
	throw std::runtime_error (std::string ("No path validation method with "
					       "name ") + type);
      }
317
318
319
320
      pathValidationType_ = type;
      pathValidationTolerance_ = tolerance;
      // If a robot is present, set path validation method
      if (robot_ && problem_) {
321
        initPathValidation();
322
323
324
      }
    }

325
326
327
328
    void ProblemSolver::initPathValidation ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      PathValidationPtr_t pathValidation =
329
        pathValidations.get (pathValidationType_)
330
331
332
333
        (robot_, pathValidationTolerance_);
      problem_->pathValidation (pathValidation);
    }

334
335
336
    void ProblemSolver::pathProjectorType (const std::string& type,
					    const value_type& tolerance)
    {
337
      if (!pathProjectors.has (type)) {
338
339
340
341
342
343
344
	throw std::runtime_error (std::string ("No path projector method with "
					       "name ") + type);
      }
      pathProjectorType_ = type;
      pathProjectorTolerance_ = tolerance;
      // If a robot is present, set path projector method
      if (robot_ && problem_) {
345
	initPathProjector ();
346
347
348
      }
    }

349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
    void ProblemSolver::addConfigValidationBuilder
    (const std::string& type, const ConfigValidationBuilder_t& builder)
    {
      configValidations.add (type, builder);
    }

    void ProblemSolver::addConfigValidation (const std::string& type)
    {
      if (!configValidations.has (type)) {
	throw std::runtime_error (std::string ("No config validation method with "
					       "name ") + type);
      }
      configValidationTypes_.push_back (type);
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      // If a robot is present, set config validation methods
      if (robot_) {
        ConfigValidationPtr_t configValidation =
	      configValidations.get (type) (robot_);
        problem_->addConfigValidation (configValidation);
      }
    }

    void ProblemSolver::clearConfigValidations ()
    {
      configValidationTypes_.clear ();
      problem_->clearConfigValidations ();
    }

377
378
379
380
381
382
383
384
385
386
387
388
    void ProblemSolver::robotType (const std::string& type)
    {
      robotType_ = type;
    }

    const std::string& ProblemSolver::robotType () const
    {
      return robotType_;
    }

    DevicePtr_t ProblemSolver::createRobot (const std::string& name)
    {
389
      RobotBuilder_t factory (robots.get (robotType_));
390
391
392
393
      assert (factory);
      return factory (name);
    }

394
395
396
397
    void ProblemSolver::robot (const DevicePtr_t& robot)
    {
      robot_ = robot;
      constraints_ = ConstraintSet::create (robot_, "Default constraint set");
398
399
400
      // Reset obstacles
      obstacleModel_ = pinocchio::GeomModelPtr_t (new GeomModel());
      obstacleData_ = pinocchio::GeomDataPtr_t (new GeomData(*obstacleModel_));
401
      resetProblem ();
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
    }

    const DevicePtr_t& ProblemSolver::robot () const
    {
      return robot_;
    }

    void ProblemSolver::initConfig (const ConfigurationPtr_t& config)
    {
      initConf_ = config;
    }

    const Configurations_t& ProblemSolver::goalConfigs () const
    {
      return goalConfigurations_;
    }

    void ProblemSolver::addGoalConfig (const ConfigurationPtr_t& config)
    {
421
      target_ = problemTarget::GoalConfigurations::create(NULL);
422
423
424
425
426
427
428
429
      goalConfigurations_.push_back (config);
    }

    void ProblemSolver::resetGoalConfigs ()
    {
      goalConfigurations_.clear ();
    }

430
431
432
433
434
435
    /* Setting goal constraint is disabled for now.
     * To re-enable it :
     * - add a function called setGoalConstraints that:
     *   - takes all the NumericalConstraintPtr_t and LockedJointPtr_t
     *   - creates a TaskTarget and fills it.
     * - remove all the addGoalConstraint
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
    void ProblemSolver::addGoalConstraint (const ConstraintPtr_t& constraint)
    {
      if (!goalConstraints_) {
        if (!robot_) throw std::logic_error ("You must provide a robot first");
        goalConstraints_ = ConstraintSet::create (robot_, "Goal constraint set");
      }
      goalConstraints_->addConstraint (constraint);
    }

    void ProblemSolver::addGoalConstraint (const LockedJointPtr_t& lj)
    {
      if (!goalConstraints_) {
        if (!robot_) throw std::logic_error ("You must provide a robot first");
        goalConstraints_ = ConstraintSet::create (robot_, "Goal constraint set");
      }
      ConfigProjectorPtr_t  configProjector = goalConstraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
454
	  (robot_, "Goal ConfigProjector", errorThreshold_, maxIterProjection_);
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
	goalConstraints_->addConstraint (configProjector);
      }
      configProjector->add (lj);
    }

    void ProblemSolver::addGoalConstraint (const std::string& constraintName,
        const std::string& functionName, const std::size_t priority)
    {
      if (!goalConstraints_) {
        if (!robot_) throw std::logic_error ("You must provide a robot first");
        goalConstraints_ = ConstraintSet::create (robot_, "Goal constraint set");
      }
      ConfigProjectorPtr_t  configProjector = goalConstraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
470
	  (robot_, constraintName, errorThreshold_, maxIterProjection_);
471
472
473
	goalConstraints_->addConstraint (configProjector);
      }
      configProjector->add (numericalConstraint (functionName),
474
			    segments_t (0), priority);
475
476
477
478
479
480
    }

    void ProblemSolver::resetGoalConstraint ()
    {
      goalConstraints_.reset ();
    }
481
    */
482

483
484
    void ProblemSolver::addConstraint (const ConstraintPtr_t& constraint)
    {
485
486
487
      if (robot_)
	constraints_->addConstraint (constraint);
      else
488
	hppDout (error, "Cannot add constraint while robot is not set");
489
490
    }

491
492
493
494
495
496
497
498
    void ProblemSolver::addLockedJoint (const LockedJointPtr_t& lj)
    {
      if (!robot_) {
	hppDout (error, "Cannot add constraint while robot is not set");
      }
      ConfigProjectorPtr_t  configProjector = constraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
499
	  (robot_, "ConfigProjector", errorThreshold_, maxIterProjection_);
500
501
502
503
504
	constraints_->addConstraint (configProjector);
      }
      configProjector->add (lj);
    }

505
506
    void ProblemSolver::resetConstraints ()
    {
507
      if (robot_) {
508
	constraints_ = ConstraintSet::create (robot_, "Default constraint set");
509
510
511
512
        if (problem_) {
          problem_->constraints (constraints_);
        }
      }
513
514
    }

515
516
    void ProblemSolver::addNumericalConstraintToConfigProjector
    (const std::string& configProjName, const std::string& constraintName,
517
     const std::size_t priority)
518
519
520
521
522
523
524
    {
      if (!robot_) {
	hppDout (error, "Cannot add constraint while robot is not set");
      }
      ConfigProjectorPtr_t  configProjector = constraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
525
	  (robot_, configProjName, errorThreshold_, maxIterProjection_);
526
527
	constraints_->addConstraint (configProjector);
      }
528
      if (!numericalConstraints.has (constraintName)) {
529
530
        std::stringstream ss; ss << "Function " << constraintName <<
                                " does not exists";
531
532
        throw std::invalid_argument (ss.str());
      }
533
      configProjector->add (numericalConstraints.get(constraintName),
534
			    segments_t (0), priority);
535
536
    }

537
538
539
540
541
542
543
544
545
546
547
548
    void ProblemSolver::addLockedJointToConfigProjector
    (const std::string& configProjName, const std::string& lockedJointName)
    {
      if (!robot_) {
	hppDout (error, "Cannot add constraint while robot is not set");
      }
      ConfigProjectorPtr_t  configProjector = constraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
	  (robot_, configProjName, errorThreshold_, maxIterProjection_);
	constraints_->addConstraint (configProjector);
      }
549
      if (!lockedJoints.has (lockedJointName)) {
550
551
552
553
        std::stringstream ss; ss << "Function " << lockedJointName <<
                                " does not exists";
        throw std::invalid_argument (ss.str());
      }
554
      configProjector->add (lockedJoints.get(lockedJointName));
555
556
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
557
    void ProblemSolver::comparisonType (const std::string& name,
558
        const ComparisonTypes_t types)
Joseph Mirabel's avatar
Joseph Mirabel committed
559
    {
560
      NumericalConstraintPtr_t nc;
561
562
563
564
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
565
566
567
568
      else
        throw std::runtime_error (name + std::string (" is neither a numerical "
              "constraint nor a locked joint"));
      nc->comparisonType (types);
Joseph Mirabel's avatar
Joseph Mirabel committed
569
570
    }

571
    void ProblemSolver::comparisonType (const std::string& name,
572
        const ComparisonType &type)
573
    {
574
      NumericalConstraintPtr_t nc;
575
576
577
578
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
579
580
581
582
583
      else
        throw std::runtime_error (name + std::string (" is neither a numerical "
              "constraint nor a locked joint"));
      ComparisonTypes_t eqtypes (nc->function().outputDerivativeSize(), type);
      nc->comparisonType (eqtypes);
584
585
    }

586
    ComparisonTypes_t ProblemSolver::comparisonType (const std::string& name) const
Joseph Mirabel's avatar
Joseph Mirabel committed
587
    {
588
      NumericalConstraintPtr_t nc;
589
590
591
592
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
593
594
595
596
      else
        throw std::runtime_error (name + std::string (" is neither a numerical "
              "constraint nor a locked joint"));
      return nc->comparisonType ();
Joseph Mirabel's avatar
Joseph Mirabel committed
597
598
    }

599
600
601
602
603
604
605
606
607
608
609
    void ProblemSolver::computeValueAndJacobian
    (const Configuration_t& configuration, vector_t& value, matrix_t& jacobian)
      const
    {
      if (!robot ()) throw std::runtime_error ("No robot loaded");
      ConfigProjectorPtr_t configProjector
	(constraints ()->configProjector ());
      if (!configProjector) {
	throw std::runtime_error ("No constraints have assigned.");
      }
      // resize value and Jacobian
610
611
      value.resize (configProjector->solver().dimension());
      size_type rows = configProjector->solver().reducedDimension();
612
613
614
615
      jacobian.resize (rows, configProjector->numberNonLockedDof ());
      configProjector->computeValueAndJacobian (configuration, value, jacobian);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
    void ProblemSolver::maxIterProjection (size_type iterations)
    {
      maxIterProjection_ = iterations;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->maxIterations (iterations);
      }
    }

    void ProblemSolver::maxIterPathPlanning (size_type iterations)
    {
      maxIterPathPlanning_ = iterations;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->maxIterations (iterations);
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
632
633
634
635
636
637
638
639
    void ProblemSolver::errorThreshold (const value_type& threshold)
    {
      errorThreshold_ = threshold;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->errorThreshold (threshold);
      }
    }

640
641
642
643
    void ProblemSolver::resetProblem ()
    {
      if (problem_)
	delete problem_;
644
645
646
647
648
649
      initializeProblem (new Problem (robot_));
    }

    void ProblemSolver::initializeProblem (ProblemPtr_t problem)
    {
      problem_ = problem;
650
      resetRoadmap ();
651
652
      // Set constraints
      problem_->constraints (constraints_);
653
654
      // Set path validation method
      PathValidationPtr_t pathValidation =
655
	pathValidations.get (pathValidationType_)
Joseph Mirabel's avatar
Joseph Mirabel committed
656
        (robot_, pathValidationTolerance_);
657
      problem_->pathValidation (pathValidation);
658
659
660
661
662
663
664
665
666
      // Set config validation methods
      for (ConfigValidationTypes_t::const_iterator it =
          configValidationTypes_.begin (); it != configValidationTypes_.end ();
          ++it)
      {
        ConfigValidationPtr_t configValidation =
	      configValidations.get (*it) (robot_);
        problem_->addConfigValidation (configValidation);
      }
667
668
      // Set obstacles
      problem_->collisionObstacles(collisionObstacles_);
669
670
671
672
      // Distance to obstacles
      distanceBetweenObjects_ = DistanceBetweenObjectsPtr_t
	(new DistanceBetweenObjects (robot_));
      distanceBetweenObjects_->obstacles(distanceObstacles_);
673
674
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
675
676
677
678
679
680
681
    void ProblemSolver::problem (ProblemPtr_t problem)
    {
      if (problem_)
        delete problem_;
      problem_ = problem;
    }

682
683
684
685
686
687
688
    void ProblemSolver::resetRoadmap ()
    {
      if (!problem_)
        throw std::runtime_error ("The problem is not defined.");
      roadmap_ = Roadmap::create (problem_->distance (), problem_->robot());
    }

689
    void ProblemSolver::createPathOptimizers ()
690
    {
691
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
692
693
694
695
      pathOptimizers_.clear();
      for (PathOptimizerTypes_t::const_iterator it =
          pathOptimizerTypes_.begin (); it != pathOptimizerTypes_.end ();
          ++it) {
696
        PathOptimizerBuilder_t createOptimizer = pathOptimizers.get (*it);
697
        pathOptimizers_.push_back (createOptimizer (*problem_));
698
699
700
      }
    }

701
702
703
    void ProblemSolver::initDistance ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
704
      DistancePtr_t dist (distances.get (distanceType_) (*problem_));
705
      problem_->distance (dist);
706
707
    }

708
    void ProblemSolver::initSteeringMethod ()
709
    {
710
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
711
      SteeringMethodPtr_t sm (
712
          steeringMethods.get (steeringMethodType_) (*problem_)
713
          );
714
      problem_->steeringMethod (sm);
715
716
717
718
    }

    void ProblemSolver::initPathProjector ()
    {
719
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
720
      PathProjectorBuilder_t createProjector =
721
        pathProjectors.get (pathProjectorType_);
722
723
724
725
726
      // The PathProjector will store a copy of the current steering method.
      // This means:
      // - when constraints are relevant, they should have been added before.
      //   TODO The path projector should update the constraint according to the path they project.
      // - the steering method type must match the path projector type.
727
      PathProjectorPtr_t pathProjector_ =
728
        createProjector (*problem_, pathProjectorTolerance_);
729
      problem_->pathProjector (pathProjector_);
730
731
    }

732
733
734
735
736
737
738
    void ProblemSolver::initProblemTarget ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      target_->problem(problem_);
      problem_->target (target_);
    }

739
740
    void ProblemSolver::initProblem ()
    {
741
742
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

743
744
      // Set shooter
      problem_->configurationShooter
745
        (configurationShooters.get (configurationShooterType_) (*problem_));
746
747
      // Set steeringMethod
      initSteeringMethod ();
748
      PathPlannerBuilder_t createPlanner = pathPlanners.get (pathPlannerType_);
749
      pathPlanner_ = createPlanner (*problem_, roadmap_);
750
      pathPlanner_->maxIterations (maxIterPathPlanning_);
751
752
753
      roadmap_ = pathPlanner_->roadmap();
      /// create Path projector
      initPathProjector ();
754
      /// create Path optimizer
755
756
      // Reset init and goal configurations
      problem_->initConfig (initConf_);
Joseph Mirabel's avatar
Joseph Mirabel committed
757
      problem_->resetGoalConfigs ();
758
759
      for (Configurations_t::const_iterator itConfig =
	     goalConfigurations_.begin ();
Joseph Mirabel's avatar
Joseph Mirabel committed
760
	   itConfig != goalConfigurations_.end (); ++itConfig) {
Joseph Mirabel's avatar
Joseph Mirabel committed
761
	problem_->addGoalConfig (*itConfig);
762
      }
763
      initProblemTarget();
764
765
766
767
768
    }

    bool ProblemSolver::prepareSolveStepByStep ()
    {
      initProblem ();
769
770
771
772
773
774
775
776
777
778
779
780
781
782

      pathPlanner_->startSolve ();
      pathPlanner_->tryDirectPath ();
      return roadmap_->pathExists ();
    }

    bool ProblemSolver::executeOneStep ()
    {
      pathPlanner_->oneStep ();
      return roadmap_->pathExists ();
    }

    void ProblemSolver::finishSolveStepByStep ()
    {
783
784
      if (!roadmap_->pathExists ())
        throw std::logic_error ("No path exists.");
785
786
787
788
      PathVectorPtr_t planned =  pathPlanner_->computePath ();
      paths_.push_back (pathPlanner_->finishSolve (planned));
    }

789
790
    void ProblemSolver::solve ()
    {
791
792
      initProblem ();

793
794
      PathVectorPtr_t path = pathPlanner_->solve ();
      paths_.push_back (path);
795
      optimizePath (path);
796
797
    }

798
799
800
    bool ProblemSolver::directPath
    (ConfigurationIn_t start, ConfigurationIn_t end, bool validate,
     std::size_t& pathId, std::string& report)
801
    {
802
      report = "";
803
804
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

805
      // Create steering method using factory
806
807
      SteeringMethodPtr_t sm (steeringMethods.get (steeringMethodType_)
          (*problem_));
808
809
      problem_->steeringMethod (sm);
      PathPtr_t dp = (*sm) (start, end);
810
811
812
813
814
      if (!dp) {
	report = "Steering method failed to build a path.";
	pathId = -1;
	return false;
      }
815
      PathPtr_t dp1, dp2;
816
      PathValidationReportPtr_t r;
817
      bool projValid = true, projected = true, pathValid = true;
818
      if (validate) {
819
820
821
822
823
824
825
826
827
828
829
830
831
        PathProjectorPtr_t proj (problem ()->pathProjector ());
        if (proj) {
          projected = proj->apply (dp, dp1);
        } else {
          dp1 = dp;
        }
	projValid = problem()->pathValidation ()->validate (dp1, false, dp2, r);
        pathValid = projValid && projected;
        if (!projValid) {
          hppDout (info, *r);
          std::ostringstream oss;
          oss << *r; report = oss.str ();
        }
832
      } else {
833
        dp2 = dp;
834
835
      }
      // Add Path in problem
836
837
838
      PathVectorPtr_t path (core::PathVector::create (dp2->outputSize (),
                            dp2->outputDerivativeSize ()));
      path->appendPath (dp2);
839
      pathId = addPath (path);
840
      return pathValid;
841
842
    }

843
    void ProblemSolver::addConfigToRoadmap (const ConfigurationPtr_t& config)
844
    {
845
      roadmap_->addNode(config);
846
    }
847

848
849
850
    void ProblemSolver::addEdgeToRoadmap (const ConfigurationPtr_t& config1,
					  const ConfigurationPtr_t& config2,
					  const PathPtr_t& path)
851
852
853
854
855
856
    {
      NodePtr_t node1, node2;
      value_type accuracy = 10e-6;
      value_type distance1, distance2;
      node1 = roadmap_->nearestNode(config1, distance1);
      node2 = roadmap_->nearestNode(config2, distance2);
857
      if (distance1 >= accuracy) {
858
	throw std::runtime_error ("No node of the roadmap contains config1");
859
      }
860
      if (distance2 >= accuracy) {
861
	throw std::runtime_error ("No node of the roadmap contains config2");
862
      }
863
      roadmap_->addEdge(node1, node2, path);
864
865
    }

866
867
868
869
870
871
872
873
874
    void ProblemSolver::interrupt ()
    {
      if (pathPlanner ()) pathPlanner ()->interrupt ();
      for (PathOptimizers_t::iterator it = pathOptimizers_.begin ();
	   it != pathOptimizers_.end (); ++it) {
	(*it)->interrupt ();
      }
    }

875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
    void ProblemSolver::addObstacle (const DevicePtr_t& device,
				     bool collision, bool distance)
    {
      device->computeForwardKinematics();
      device->updateGeometryPlacements();
      const std::string& prefix = device->name();

      // Detach objects from joints
      pinocchio::DeviceObjectVector& objects = device->objectVector();
      for (pinocchio::DeviceObjectVector::iterator itObj = objects.begin();
          itObj != objects.end(); ++itObj) {
        addObstacle (prefix + (*itObj)->name (),
            *(*itObj)->fcl (),
            collision, distance);
      }
    }

892
    void ProblemSolver::addObstacle (const CollisionObjectPtr_t& object,
893
894
				     bool collision, bool distance)
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
895
896
897
898
899
900
901
902
903
      // FIXME propagate object->mesh_path ?
      addObstacle(object->name(), *object->fcl(), collision, distance);
    }

    void ProblemSolver::addObstacle (const std::string& name,
                                     /*const*/ FclCollisionObject &inObject,
				     bool collision, bool distance)
    {
      if (obstacleModel_->existGeometryName(name)) {
904
905
        HPP_THROW(std::runtime_error, "object with name " << name
            << " already added! Choose another name (prefix).");
Joseph Mirabel's avatar
Joseph Mirabel committed
906
907
908
      }

      se3::GeomIndex id = obstacleModel_->addGeometryObject(se3::GeometryObject(
909
            name, obsModel.getFrameId("obstacle_frame", se3::BODY), 0,
Joseph Mirabel's avatar
Joseph Mirabel committed
910
911
            inObject.collisionGeometry(),
            se3::toPinocchioSE3(inObject.getTransform()),
912
913
            "",
            vector3_t::Ones()),
914
          obsModel);
Joseph Mirabel's avatar
Joseph Mirabel committed
915
916
917
918
919
920
921
922
923
924
925
      // Update obstacleData_
      // FIXME This should be done in Pinocchio
      {
        se3::GeometryModel& model = *obstacleModel_;
        se3::GeometryData& data = *obstacleData_;
        data.oMg.resize(model.ngeoms);
        //data.activeCollisionPairs.resize(model.collisionPairs.size(), true)
        //data.distance_results(model.collisionPairs.size())
        //data.collision_results(model.collisionPairs.size())
        //data.radius()
        data.collisionObjects.push_back (fcl::CollisionObject(
926
              model.geometryObjects[id].fcl));
Joseph Mirabel's avatar
Joseph Mirabel committed
927
928
929
930
931
        data.oMg[id] =  model.geometryObjects[id].placement;
        data.collisionObjects[id].setTransform( se3::toFclTransform3f(data.oMg[id]) );
      }
      CollisionObjectPtr_t object (
          new CollisionObject(obstacleModel_,obstacleData_,id));
932

933
      if (collision){
934
        collisionObstacles_.push_back (object);
935
936
        resetRoadmap ();
      }
937
      if (distance)
938
        distanceObstacles_.push_back (object);
939
      if (problem ())
940
941
942
943
        problem ()->addObstacle (object);
      if (distanceBetweenObjects_) {
	distanceBetweenObjects_->addObstacle (object);
      }
944
945
    }

946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
    void ProblemSolver::removeObstacle (const std::string& name)
    {
      if (!obstacleModel_->existGeometryName(name)) {
        HPP_THROW(std::invalid_argument, "No obstacle with name " << name);
      }
      se3::GeomIndex id = obstacleModel_->getGeometryId(name);

      // Update obstacle model
      remove(obstacleModel_->geometryObjects, id);
      obstacleModel_->ngeoms--;
      remove(obstacleData_->oMg, id);
      remove(obstacleData_->collisionObjects, id);

      remove(collisionObstacles_, id);
      remove(distanceObstacles_, id);
      resetProblem(); // resets problem_ and distanceBetweenObjects_
      resetRoadmap();
    }

    void ProblemSolver::cutObstacle (const std::string& name,
                                     const fcl::AABB& aabb)
    {
      if (!obstacleModel_->existGeometryName(name)) {
        HPP_THROW(std::invalid_argument, "No obstacle with name " << name);
      }
      se3::GeomIndex id = obstacleModel_->getGeometryId(name);

      fcl::CollisionObject& fclobj = obstacleData_->collisionObjects[id];
      fclobj.computeAABB();
      if (!fclobj.getAABB().overlap(aabb)) {
        // No intersection. Geom should be removed.
        removeObstacle(name);
        return;
      }
      fcl::CollisionGeometryPtr_t fclgeom = obstacleModel_->geometryObjects[id].fcl;
      fcl::CollisionGeometryPtr_t newgeom (extract(fclgeom.get(), fclobj.getTransform(), aabb));
      if (!newgeom) {
        // No intersection. Geom should be removed.
        removeObstacle(name);
      } else {
        obstacleModel_->geometryObjects[id].fcl = newgeom;
        obstacleData_->collisionObjects[id] =
          fcl::CollisionObject(newgeom, se3::toFclTransform3f(obstacleData_->oMg[id]));
      }
    }

992
993
994
995
996
997
998
    void ProblemSolver::removeObstacleFromJoint
    (const std::string& obstacleName, const std::string& jointName)
    {
      if (!robot_) {
	throw std::runtime_error ("No robot defined.");
      }
      JointPtr_t joint = robot_->getJointByName (jointName);
999
1000
      if (!joint) {
	std::ostringstream oss;
For faster browsing, not all history is shown. View entire blame