problem-solver.cc 40.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//
// 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
19
20
#include <hpp/core/problem-solver.hh>

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

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

25
#include <pinocchio/algorithm/frames.hpp>
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
#include <hpp/pinocchio/joint-collection.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
34

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

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

79
80
#include "../src/path-validation/no-validation.hh"

81
82
namespace hpp {
  namespace core {
83
    using boost::bind;
84
85
    using pinocchio::GeomIndex;

86
    using pinocchio::Model;
87
88
89
    using pinocchio::ModelPtr_t;
    using pinocchio::Data;
    using pinocchio::DataPtr_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
90
91
92
93
94
95
    using pinocchio::GeomModel;
    using pinocchio::GeomModelPtr_t;
    using pinocchio::GeomData;
    using pinocchio::GeomDataPtr_t;
    using pinocchio::CollisionObject;

96
    namespace {
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
      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);
      }
119
120
    }

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

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

138
139
140
141
142
143
144
145
146
147
148
149
    pathValidation::DiscretizedPtr_t createDiscretizedJointBoundAndCollisionChecking (
        const DevicePtr_t& robot, const value_type& stepSize)
    {
      using namespace pathValidation;
      DiscretizedPtr_t pv (Discretized::create (stepSize));
      JointBoundValidationPtr_t jbv (JointBoundValidation::create (robot));
      pv->add (jbv);
      CollisionValidationPtr_t cv (CollisionValidation::create (robot));
      pv->add (cv);
      return pv;
    }

150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
    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;
    }

165
166
167
168
169
170
171
    configurationShooter::UniformPtr_t createUniformConfigShooter (const Problem& p)
    {
      configurationShooter::UniformPtr_t ptr = configurationShooter::Uniform::create(p.robot());
      ptr->sampleExtraDOF(p.getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
      return ptr;
    }

172
173
174
175
176
177
178
179
180
181
182
183
    ProblemSolverPtr_t ProblemSolver::latest_ = 0x0;
    ProblemSolverPtr_t ProblemSolver::create ()
    {
      latest_ = new ProblemSolver ();
      return latest_;
    }

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

184
    ProblemSolver::ProblemSolver () :
185
      constraints_ (), robot_ (), problem_ (), pathPlanner_ (),
186
187
      roadmap_ (), paths_ (),
      pathProjectorType_ ("None"), pathProjectorTolerance_ (0.2),
188
      pathPlannerType_ ("DiffusingPlanner"),
189
      target_ (problemTarget::GoalConfigurations::create(ProblemPtr_t())),
190
      initConf_ (), goalConfigurations_ (),
191
      robotType_ ("hpp::pinocchio::Device"),
192
      configurationShooterType_ ("Uniform"),
193
      distanceType_("WeighedDistance"),
194
      steeringMethodType_ ("Straight"),
195
      pathOptimizerTypes_ (), pathOptimizers_ (),
196
      pathValidationType_ ("Discretized"), pathValidationTolerance_ (0.05),
197
      configValidationTypes_ (),
Joseph Mirabel's avatar
Joseph Mirabel committed
198
      collisionObstacles_ (), distanceObstacles_ (),
199
      obstacleRModel_ (new Model()),
200
      obstacleRData_ (),
201
202
      obstacleModel_ (new GeomModel()),
      obstacleData_ (new GeomData(*obstacleModel_)),
203
204
      errorThreshold_ (1e-4), maxIterProjection_ (20),
      maxIterPathPlanning_ (std::numeric_limits
205
			    <unsigned long int>::max ()),
206
207
      timeOutPathPlanning_(std::numeric_limits<double>::infinity()),
      
208
      passiveDofsMap_ (), comcMap_ (),
209
      distanceBetweenObjects_ ()
210
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
211
      obstacleRModel_->addFrame(::pinocchio::Frame("obstacle_frame", 0, 0, Transform3f::Identity(), ::pinocchio::BODY));
212
213
214
      obstacleRData_.reset (new Data (*obstacleRModel_));


215
216
217
218
      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
219
      pathPlanners.add ("kPRM*", pathPlanner::kPrmStar::createWithRoadmap);
Joseph Mirabel's avatar
Joseph Mirabel committed
220

221
      configurationShooters.add ("Uniform" , createUniformConfigShooter);
222
      configurationShooters.add ("Gaussian", createGaussianConfigShooter);
223
224

      distances.add ("Weighed",         WeighedDistance::createFromProblem);
225
      distances.add ("ReedsShepp",      bind (distance::ReedsShepp::create, _1));
226
227
      distances.add ("Kinodynamic",     KinodynamicDistance::createFromProblem);

Joseph Mirabel's avatar
Joseph Mirabel committed
228

229
      steeringMethods.add ("Straight",   Factory<steeringMethod::Straight>::create);
230
      steeringMethods.add ("ReedsShepp", steeringMethod::ReedsShepp::createWithGuess);
231
      steeringMethods.add ("Kinodynamic", steeringMethod::Kinodynamic::create);
232
233
234
      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
235

Joseph Mirabel's avatar
Joseph Mirabel committed
236
      // Store path optimization methods in map.
237
      pathOptimizers.add ("RandomShortcut",     pathOptimization::RandomShortcut::create);
238
      pathOptimizers.add ("SimpleShortcut",     pathOptimization::SimpleShortcut::create);
239
240
241
      pathOptimizers.add ("PartialShortcut",    pathOptimization::PartialShortcut::create);
      pathOptimizers.add ("SimpleTimeParameterization", pathOptimization::SimpleTimeParameterization::create);

242
      // Store path validation methods in map.
243
244
      pathValidations.add ("NoValidation",
                           pathValidation::NoValidation::create);
245
      pathValidations.add ("Discretized", pathValidation::createDiscretizedCollisionChecking);
246
      pathValidations.add ("DiscretizedCollision", pathValidation::createDiscretizedCollisionChecking);
247
      pathValidations.add ("DiscretizedJointBound", pathValidation::createDiscretizedJointBound);
248
      pathValidations.add ("DiscretizedCollisionAndJointBound", createDiscretizedJointBoundAndCollisionChecking);
249
250
      pathValidations.add ("Progressive", continuousValidation::Progressive::create);
      pathValidations.add ("Dichotomy",   continuousValidation::Dichotomy::create);
Joseph Mirabel's avatar
Joseph Mirabel committed
251

252
253
254
255
256
257
258
259
      // 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");

260
      // Store path projector methods in map.
261
262
263
264
265
      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);
266
267
268
269
270
271
    }

    ProblemSolver::~ProblemSolver ()
    {
    }

272
273
    void ProblemSolver::distanceType (const std::string& type)
    {
274
      if (!distances.has (type)) {
275
276
277
278
    throw std::runtime_error (std::string ("No distance method with name ") +
                  type);
      }
      distanceType_ = type;
Joseph Mirabel's avatar
Joseph Mirabel committed
279
280
      // TODO
      // initDistance ();
281
282
    }

283
284
    void ProblemSolver::steeringMethodType (const std::string& type)
    {
285
      if (!steeringMethods.has (type)) {
286
287
288
289
	throw std::runtime_error (std::string ("No steering method with name ") +
				  type);
      }
      steeringMethodType_ = type;
290
      if (problem_) initSteeringMethod();
291
292
    }

293
294
    void ProblemSolver::pathPlannerType (const std::string& type)
    {
295
      if (!pathPlanners.has (type)) {
296
297
298
	throw std::runtime_error (std::string ("No path planner with name ") +
				  type);
      }
299
300
301
      pathPlannerType_ = type;
    }

302
303
    void ProblemSolver::configurationShooterType (const std::string& type)
    {
304
      if (!configurationShooters.has (type)) {
305
306
307
308
    throw std::runtime_error (std::string ("No configuration shooter with name ") +
                  type);
      }
      configurationShooterType_ = type;
309
310
      if (robot_ && problem_) {
        problem_->configurationShooter
311
          (configurationShooters.get (configurationShooterType_) (*problem_));
312
      }
313
314
    }

315
    void ProblemSolver::addPathOptimizer (const std::string& type)
316
    {
317
      if (!pathOptimizers.has (type)) {
318
319
320
	throw std::runtime_error (std::string ("No path optimizer with name ") +
				  type);
      }
321
322
323
324
325
326
      pathOptimizerTypes_.push_back (type);
    }

    void ProblemSolver::clearPathOptimizers ()
    {
      pathOptimizerTypes_.clear ();
327
      pathOptimizers_.clear ();
328
329
330
331
332
333
334
335
336
337
    }

    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);
      }
338
339
    }

340
341
342
    void ProblemSolver::pathValidationType (const std::string& type,
					    const value_type& tolerance)
    {
343
      if (!pathValidations.has (type)) {
344
345
346
	throw std::runtime_error (std::string ("No path validation method with "
					       "name ") + type);
      }
347
348
349
350
      pathValidationType_ = type;
      pathValidationTolerance_ = tolerance;
      // If a robot is present, set path validation method
      if (robot_ && problem_) {
351
        initPathValidation();
352
353
354
      }
    }

355
356
357
358
    void ProblemSolver::initPathValidation ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      PathValidationPtr_t pathValidation =
359
        pathValidations.get (pathValidationType_)
360
361
362
363
        (robot_, pathValidationTolerance_);
      problem_->pathValidation (pathValidation);
    }

364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
    void ProblemSolver::initConfigValidation ()
    {
      if (!robot_) throw std::logic_error ("You must provide a robot first");
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      problem_->resetConfigValidations();
      for (ConfigValidationTypes_t::const_iterator it =
          configValidationTypes_.begin (); it != configValidationTypes_.end ();
          ++it)
      {
        ConfigValidationPtr_t configValidation =
          configValidations.get (*it) (robot_);
        problem_->addConfigValidation (configValidation);
      }
    }

379
380
381
382
383
384
385
    void ProblemSolver::initValidations ()
    {
      initPathValidation ();
      initConfigValidation ();
      problem_->collisionObstacles(collisionObstacles_);
    }

386
387
388
    void ProblemSolver::pathProjectorType (const std::string& type,
					    const value_type& tolerance)
    {
389
      if (!pathProjectors.has (type)) {
390
391
392
393
394
395
396
	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_) {
397
	initPathProjector ();
398
399
400
      }
    }

401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
    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
416
      initConfigValidation ();
417
418
419
420
421
422
423
424
    }

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

425
426
427
428
429
430
431
432
433
434
435
436
    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)
    {
437
      RobotBuilder_t factory (robots.get (robotType_));
438
439
440
441
      assert (factory);
      return factory (name);
    }

442
443
444
445
    void ProblemSolver::robot (const DevicePtr_t& robot)
    {
      robot_ = robot;
      constraints_ = ConstraintSet::create (robot_, "Default constraint set");
446
      // Reset obstacles
447
      obstacleRModel_.reset(new Model());
Joseph Mirabel's avatar
Joseph Mirabel committed
448
      obstacleRModel_->addFrame(::pinocchio::Frame("obstacle_frame", 0, 0, Transform3f::Identity(), ::pinocchio::BODY));
449
450
451
      obstacleRData_.reset (new Data(*obstacleRModel_));
      obstacleModel_.reset (new GeomModel());
      obstacleData_ .reset (new GeomData(*obstacleModel_));
452
      resetProblem ();
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
    }

    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)
    {
472
      target_ = problemTarget::GoalConfigurations::create(ProblemPtr_t());
473
474
475
476
477
478
479
480
      goalConfigurations_.push_back (config);
    }

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

481
482
483
    /* Setting goal constraint is disabled for now.
     * To re-enable it :
     * - add a function called setGoalConstraints that:
484
     *   - takes all the constraints::ImplicitPtr_t and LockedJointPtr_t
485
486
     *   - creates a TaskTarget and fills it.
     * - remove all the addGoalConstraint
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
    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
505
	  (robot_, "Goal ConfigProjector", errorThreshold_, maxIterProjection_);
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
	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
521
	  (robot_, constraintName, errorThreshold_, maxIterProjection_);
522
523
524
	goalConstraints_->addConstraint (configProjector);
      }
      configProjector->add (numericalConstraint (functionName),
525
			    segments_t (0), priority);
526
527
528
529
530
531
    }

    void ProblemSolver::resetGoalConstraint ()
    {
      goalConstraints_.reset ();
    }
532
    */
533

534
535
    void ProblemSolver::addConstraint (const ConstraintPtr_t& constraint)
    {
536
537
538
      if (robot_)
	constraints_->addConstraint (constraint);
      else
539
	hppDout (error, "Cannot add constraint while robot is not set");
540
541
    }

542
543
544
545
546
547
548
549
    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
550
	  (robot_, "ConfigProjector", errorThreshold_, maxIterProjection_);
551
552
553
554
555
	constraints_->addConstraint (configProjector);
      }
      configProjector->add (lj);
    }

556
557
    void ProblemSolver::resetConstraints ()
    {
558
      if (robot_) {
559
	constraints_ = ConstraintSet::create (robot_, "Default constraint set");
560
561
562
563
        if (problem_) {
          problem_->constraints (constraints_);
        }
      }
564
565
    }

566
567
    void ProblemSolver::addNumericalConstraintToConfigProjector
    (const std::string& configProjName, const std::string& constraintName,
568
     const std::size_t priority)
569
570
571
572
573
574
575
    {
      if (!robot_) {
	hppDout (error, "Cannot add constraint while robot is not set");
      }
      ConfigProjectorPtr_t  configProjector = constraints_->configProjector ();
      if (!configProjector) {
	configProjector = ConfigProjector::create
576
	  (robot_, configProjName, errorThreshold_, maxIterProjection_);
577
578
	constraints_->addConstraint (configProjector);
      }
579
      if (!numericalConstraints.has (constraintName)) {
580
581
        std::stringstream ss; ss << "Function " << constraintName <<
                                " does not exists";
582
583
        throw std::invalid_argument (ss.str());
      }
584
      configProjector->add (numericalConstraints.get(constraintName),
585
			    segments_t (0), priority);
586
587
    }

588
589
590
591
592
593
594
595
596
597
598
599
    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);
      }
600
      if (!lockedJoints.has (lockedJointName)) {
601
602
603
604
        std::stringstream ss; ss << "Function " << lockedJointName <<
                                " does not exists";
        throw std::invalid_argument (ss.str());
      }
605
      configProjector->add (lockedJoints.get(lockedJointName));
606
607
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
608
    void ProblemSolver::comparisonType (const std::string& name,
609
        const ComparisonTypes_t types)
Joseph Mirabel's avatar
Joseph Mirabel committed
610
    {
611
      constraints::ImplicitPtr_t nc;
612
613
614
615
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
616
617
618
619
      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
620
621
    }

622
    void ProblemSolver::comparisonType (const std::string& name,
623
        const ComparisonType &type)
624
    {
625
      constraints::ImplicitPtr_t nc;
626
627
628
629
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
630
631
632
633
634
      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);
635
636
    }

637
    ComparisonTypes_t ProblemSolver::comparisonType (const std::string& name) const
Joseph Mirabel's avatar
Joseph Mirabel committed
638
    {
639
      constraints::ImplicitPtr_t nc;
640
641
642
643
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
      else if (lockedJoints.has (name))
        nc = lockedJoints.get(name);
644
645
646
647
      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
648
649
    }

650
651
652
653
654
655
656
657
658
659
660
    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
661
662
      value.resize (configProjector->solver().dimension());
      size_type rows = configProjector->solver().reducedDimension();
663
      jacobian.resize (rows, configProjector->numberFreeVariables ());
664
665
666
      configProjector->computeValueAndJacobian (configuration, value, jacobian);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
    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
683
684
685
686
687
688
689
690
    void ProblemSolver::errorThreshold (const value_type& threshold)
    {
      errorThreshold_ = threshold;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->errorThreshold (threshold);
      }
    }

691
692
    void ProblemSolver::resetProblem ()
    {
693
      initializeProblem (Problem::create(robot_));
694
695
696
697
698
    }

    void ProblemSolver::initializeProblem (ProblemPtr_t problem)
    {
      problem_ = problem;
699
      resetRoadmap ();
700
701
      // Set constraints
      problem_->constraints (constraints_);
702
      // Set path validation method
703
      initPathValidation ();
704
      // Set config validation methods
705
      initConfigValidation ();
706
707
      // Set obstacles
      problem_->collisionObstacles(collisionObstacles_);
708
709
710
711
      // Distance to obstacles
      distanceBetweenObjects_ = DistanceBetweenObjectsPtr_t
	(new DistanceBetweenObjects (robot_));
      distanceBetweenObjects_->obstacles(distanceObstacles_);
712
713
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
714
715
716
717
718
    void ProblemSolver::problem (ProblemPtr_t problem)
    {
      problem_ = problem;
    }

719
720
721
722
723
724
725
    void ProblemSolver::resetRoadmap ()
    {
      if (!problem_)
        throw std::runtime_error ("The problem is not defined.");
      roadmap_ = Roadmap::create (problem_->distance (), problem_->robot());
    }

726
    void ProblemSolver::createPathOptimizers ()
727
    {
728
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
729
730
731
732
      pathOptimizers_.clear();
      for (PathOptimizerTypes_t::const_iterator it =
          pathOptimizerTypes_.begin (); it != pathOptimizerTypes_.end ();
          ++it) {
733
        PathOptimizerBuilder_t createOptimizer = pathOptimizers.get (*it);
734
        pathOptimizers_.push_back (createOptimizer (*problem_));
735
736
737
      }
    }

738
739
740
    void ProblemSolver::initDistance ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
741
      DistancePtr_t dist (distances.get (distanceType_) (*problem_));
742
      problem_->distance (dist);
743
744
    }

stevet's avatar
2778843    
stevet committed
745

746
    void ProblemSolver::initSteeringMethod ()
747
    {
748
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
749
      SteeringMethodPtr_t sm (
750
          steeringMethods.get (steeringMethodType_) (*problem_)
751
          );
752
      problem_->steeringMethod (sm);
753
754
755
756
    }

    void ProblemSolver::initPathProjector ()
    {
757
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
758
      PathProjectorBuilder_t createProjector =
759
        pathProjectors.get (pathProjectorType_);
760
761
762
763
764
      // 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.
765
      PathProjectorPtr_t pathProjector_ =
766
        createProjector (*problem_, pathProjectorTolerance_);
767
      problem_->pathProjector (pathProjector_);
768
769
    }

770
771
772
773
774
775
776
    void ProblemSolver::initProblemTarget ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      target_->problem(problem_);
      problem_->target (target_);
    }

777
778
    void ProblemSolver::initProblem ()
    {
779
780
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

stevet's avatar
2778843    
stevet committed
781

782
783
      // Set shooter
      problem_->configurationShooter
784
        (configurationShooters.get (configurationShooterType_) (*problem_));
785
786
      // Set steeringMethod
      initSteeringMethod ();
787
      PathPlannerBuilder_t createPlanner = pathPlanners.get (pathPlannerType_);
788
      pathPlanner_ = createPlanner (*problem_, roadmap_);
789
      pathPlanner_->maxIterations (maxIterPathPlanning_);
790
      pathPlanner_->timeOut(timeOutPathPlanning_);
791
792
793
      roadmap_ = pathPlanner_->roadmap();
      /// create Path projector
      initPathProjector ();
794
      /// create Path optimizer
795
796
      // Reset init and goal configurations
      problem_->initConfig (initConf_);
Joseph Mirabel's avatar
Joseph Mirabel committed
797
      problem_->resetGoalConfigs ();
798
799
      for (Configurations_t::const_iterator itConfig =
	     goalConfigurations_.begin ();
Joseph Mirabel's avatar
Joseph Mirabel committed
800
	   itConfig != goalConfigurations_.end (); ++itConfig) {
Joseph Mirabel's avatar
Joseph Mirabel committed
801
	problem_->addGoalConfig (*itConfig);
802
      }
803
      initProblemTarget();
804
805
806
807
808
    }

    bool ProblemSolver::prepareSolveStepByStep ()
    {
      initProblem ();
809
810

      pathPlanner_->startSolve ();
811
      pathPlanner_->tryConnectInitAndGoals ();
812
813
814
815
816
817
818
819
820
821
822
      return roadmap_->pathExists ();
    }

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

    void ProblemSolver::finishSolveStepByStep ()
    {
823
824
      if (!roadmap_->pathExists ())
        throw std::logic_error ("No path exists.");
825
826
827
828
      PathVectorPtr_t planned =  pathPlanner_->computePath ();
      paths_.push_back (pathPlanner_->finishSolve (planned));
    }

829
830
    void ProblemSolver::solve ()
    {
831
832
      initProblem ();

833
834
      PathVectorPtr_t path = pathPlanner_->solve ();
      paths_.push_back (path);
835
      optimizePath (path);
836
837
    }

838
839
840
    bool ProblemSolver::directPath
    (ConfigurationIn_t start, ConfigurationIn_t end, bool validate,
     std::size_t& pathId, std::string& report)
841
    {
842
      report = "";
843
844
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

845
846
      // Get steering method from problem
      SteeringMethodPtr_t sm (problem_->steeringMethod());
847
      PathPtr_t dp = (*sm) (start, end);
848
849
850
851
852
      if (!dp) {
	report = "Steering method failed to build a path.";
	pathId = -1;
	return false;
      }
853
      PathPtr_t dp1, dp2;
854
      PathValidationReportPtr_t r;
855
      bool projValid = true, projected = true, pathValid = true;
856
      if (validate) {
857
858
859
860
861
862
863
864
865
        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) {
866
867
868
869
870
871
          if (r) {
            std::ostringstream oss;
            oss << *r; report = oss.str ();
          } else {
            report = "No path validation report.";
          }
872
        }
873
      } else {
874
        dp2 = dp;
875
876
      }
      // Add Path in problem
877
878
879
      PathVectorPtr_t path (core::PathVector::create (dp2->outputSize (),
                            dp2->outputDerivativeSize ()));
      path->appendPath (dp2);
880
      pathId = addPath (path);
881
      return pathValid;
882
883
    }

884
    void ProblemSolver::addConfigToRoadmap (const ConfigurationPtr_t& config)
885
    {
886
      roadmap_->addNode(config);
887
    }
888

889
890
891
    void ProblemSolver::addEdgeToRoadmap (const ConfigurationPtr_t& config1,
					  const ConfigurationPtr_t& config2,
					  const PathPtr_t& path)
892
893
894
895
896
897
    {
      NodePtr_t node1, node2;
      value_type accuracy = 10e-6;
      value_type distance1, distance2;
      node1 = roadmap_->nearestNode(config1, distance1);
      node2 = roadmap_->nearestNode(config2, distance2);
898
      if (distance1 >= accuracy) {
899
	throw std::runtime_error ("No node of the roadmap contains config1");
900
      }
901
      if (distance2 >= accuracy) {
902
	throw std::runtime_error ("No node of the roadmap contains config2");
903
      }
904
      roadmap_->addEdge(node1, node2, path);
905
906
    }

907
908
909
910
911
912
913
914
915
    void ProblemSolver::interrupt ()
    {
      if (pathPlanner ()) pathPlanner ()->interrupt ();
      for (PathOptimizers_t::iterator it = pathOptimizers_.begin ();
	   it != pathOptimizers_.end (); ++it) {
	(*it)->interrupt ();
      }
    }

916
917
918
919
    void ProblemSolver::addObstacle (const DevicePtr_t& device,
				     bool collision, bool distance)
    {
      device->computeForwardKinematics();
920
      device->computeFramesForwardKinematics();
921
922
923
      device->updateGeometryPlacements();
      const std::string& prefix = device->name();

924
925
926
927
      const Model& m = device->model();
      const Data & d = device->data ();
      for (std::size_t i = 1; i < m.frames.size(); ++i)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
928
929
930
        const ::pinocchio::Frame& frame = m.frames[i];
        // ::pinocchio::FrameType type = (frame.type == ::pinocchio::JOINT ? ::pinocchio::FIXED_JOINT : frame.type);
        obstacleRModel_->addFrame (::pinocchio::Frame (
931
932
933
934
935
936
937
938
939
              prefix + frame.name,
              0,
              0, // TODO Keep frame hierarchy
              d.oMf[i],
              // type
              frame.type
              ));
      }
      obstacleRData_.reset (new Data(*obstacleRModel_));
Joseph Mirabel's avatar
Joseph Mirabel committed
940
      ::pinocchio::framesForwardKinematics (*obstacleRModel_, *obstacleRData_, vector_t::Zero(0).eval());
941

942
      // Detach objects from joints
943
944
945
      for (size_type i = 0; i < device->nbObjects(); ++i) {
        CollisionObjectPtr_t obj = device->objectAt (i);
        addObstacle (prefix + obj->name (), *obj->fcl (), collision, distance);
946
947
948
      }
    }

949
    void ProblemSolver::addObstacle (const CollisionObjectPtr_t& object,
950
951
				     bool collision, bool distance)
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
952
953
954
955
956
957
958
959
960
      // 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)) {
961
962
        HPP_THROW(std::runtime_error, "object with name " << name
            << " already added! Choose another name (prefix).");
Joseph Mirabel's avatar
Joseph Mirabel committed
963
964
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
965
      ::pinocchio::GeomIndex id = obstacleModel_->addGeometryObject(::pinocchio::GeometryObject(
966
            name, 1, 0,
Joseph Mirabel's avatar
Joseph Mirabel committed
967
            inObject.collisionGeometry(),
Joseph Mirabel's avatar
Joseph Mirabel committed
968
            ::pinocchio::toPinocchioSE3(inObject.getTransform()),
969
970
            "",
            vector3_t::Ones()),
971
          *obstacleRModel_);
Joseph Mirabel's avatar
Joseph Mirabel committed
972
973
974
      // Update obstacleData_
      // FIXME This should be done in Pinocchio
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
975
976
        ::pinocchio::GeometryModel& model = *obstacleModel_;
        ::pinocchio::GeometryData& data = *obstacleData_;
Joseph Mirabel's avatar
Joseph Mirabel committed
977
978
979
980
981
982
        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(
983
              model.geometryObjects[id].fcl));
Joseph Mirabel's avatar
Joseph Mirabel committed
984
        data.oMg[id] =  model.geometryObjects[id].placement;
Joseph Mirabel's avatar
Joseph Mirabel committed
985
        data.collisionObjects[id].setTransform( ::pinocchio::toFclTransform3f(data.oMg[id]) );
Joseph Mirabel's avatar
Joseph Mirabel committed
986
987
988
      }
      CollisionObjectPtr_t object (
          new CollisionObject(obstacleModel_,obstacleData_,id));
989

990
      if (collision){
991
        collisionObstacles_.push_back (object);
992
993
        resetRoadmap ();
      }
994
      if (distance)
995
        distanceObstacles_.push_back (object);
996
      if (problem ())
997
998
999
1000
        problem ()->addObstacle (object);
      if (distanceBetweenObjects_) {
	distanceBetweenObjects_->addObstacle (object);
      }