problem-solver.cc 40 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>
Joseph Mirabel's avatar
Joseph Mirabel committed
52
#include <hpp/core/path-planner/bi-rrt-star.hh>
Florent Lamiraux's avatar
Florent Lamiraux committed
53
#include <hpp/core/path-planner/k-prm-star.hh>
54
#include <hpp/core/path-projector/global.hh>
55
56
#include <hpp/core/path-projector/dichotomy.hh>
#include <hpp/core/path-projector/progressive.hh>
57
#include <hpp/core/path-projector/recursive-hermite.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
58
#include <hpp/core/path-optimization/partial-shortcut.hh>
59
#include <hpp/core/path-optimization/random-shortcut.hh>
60
#include <hpp/core/path-optimization/simple-shortcut.hh>
61
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
62
63
#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
64
#include <hpp/core/path-validation-report.hh>
65
// #include <hpp/core/problem-target/task-target.hh>
66
#include <hpp/core/problem-target/goal-configurations.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
67
#include <hpp/core/roadmap.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
68
#include <hpp/core/steering-method/dubins.hh>
69
#include <hpp/core/steering-method/hermite.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
70
#include <hpp/core/steering-method/reeds-shepp.hh>
stevet's avatar
2778843    
stevet committed
71
#include <hpp/core/steering-method/steering-kinodynamic.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
72
73
#include <hpp/core/steering-method/snibud.hh>
#include <hpp/core/steering-method/straight.hh>
74
#include <hpp/core/visibility-prm-planner.hh>
75
#include <hpp/core/weighed-distance.hh>
76
77
#include <hpp/core/collision-validation.hh>
#include <hpp/core/joint-bound-validation.hh>
78
#include <hpp/core/kinodynamic-distance.hh>
79

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

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

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

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

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

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

139
140
141
142
143
144
145
146
147
148
149
150
    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;
    }

151
    template <typename T>
152
153
154
155
    boost::shared_ptr<T> createFromRobot (const ProblemConstPtr_t& p)
    {
      return T::create(p->robot());
    }
156

157
158
    configurationShooter::GaussianPtr_t createGaussianConfigShooter
    (const ProblemConstPtr_t& p)
159
    {
160
161
      configurationShooter::GaussianPtr_t ptr
	(configurationShooter::Gaussian::create(p->robot()));
162
      static const std::string center = "ConfigurationShooter/Gaussian/center";
163
164
      static const std::string useVel = "ConfigurationShooter/Gaussian/useRobotVelocity";
      static const std::string stdDev = "ConfigurationShooter/Gaussian/standardDeviation";
165
      if (p->getParameter(useVel).boolValue())
166
      {
167
168
        ptr->sigmas (p->robot()->currentVelocity());
	ptr->center(p->getParameter(center).vectorValue());
169
      }
170
171
      else if (p->parameters.has(stdDev))
        ptr->sigma (p->getParameter (stdDev).floatValue());
172
173
174
      return ptr;
    }

175
    configurationShooter::UniformPtr_t createUniformConfigShooter (const ProblemConstPtr_t& p)
176
    {
177
178
179
180
      configurationShooter::UniformPtr_t ptr
	(configurationShooter::Uniform::create(p->robot()));
      ptr->sampleExtraDOF(p->getParameter
			  ("ConfigurationShooter/sampleExtraDOF").boolValue());
181
182
183
      return ptr;
    }

184
185
    ProblemSolverPtr_t ProblemSolver::create ()
    {
186
      return ProblemSolverPtr_t(new ProblemSolver ());
187
188
    }

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


220
221
222
223
      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
224
      pathPlanners.add ("kPRM*", pathPlanner::kPrmStar::createWithRoadmap);
Joseph Mirabel's avatar
Joseph Mirabel committed
225
      pathPlanners.add ("BiRRT*", pathPlanner::BiRrtStar::createWithRoadmap);
Joseph Mirabel's avatar
Joseph Mirabel committed
226

227
      configurationShooters.add ("Uniform" , createUniformConfigShooter);
228
      configurationShooters.add ("Gaussian", createGaussianConfigShooter);
229
230

      distances.add ("Weighed",         WeighedDistance::createFromProblem);
231
      distances.add ("ReedsShepp",      bind (distance::ReedsShepp::create, _1));
232
233
      distances.add ("Kinodynamic",     KinodynamicDistance::createFromProblem);

Joseph Mirabel's avatar
Joseph Mirabel committed
234

235
      steeringMethods.add ("Straight",   Factory<steeringMethod::Straight>::create);
236
      steeringMethods.add ("ReedsShepp", steeringMethod::ReedsShepp::createWithGuess);
237
      steeringMethods.add ("Kinodynamic", steeringMethod::Kinodynamic::create);
238
239
240
      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
241

Joseph Mirabel's avatar
Joseph Mirabel committed
242
      // Store path optimization methods in map.
243
      pathOptimizers.add ("RandomShortcut",     pathOptimization::RandomShortcut::create);
244
      pathOptimizers.add ("SimpleShortcut",     pathOptimization::SimpleShortcut::create);
245
246
247
      pathOptimizers.add ("PartialShortcut",    pathOptimization::PartialShortcut::create);
      pathOptimizers.add ("SimpleTimeParameterization", pathOptimization::SimpleTimeParameterization::create);

248
      // Store path validation methods in map.
249
250
      pathValidations.add ("NoValidation",
                           pathValidation::NoValidation::create);
251
      pathValidations.add ("Discretized", pathValidation::createDiscretizedCollisionChecking);
252
      pathValidations.add ("DiscretizedCollision", pathValidation::createDiscretizedCollisionChecking);
253
      pathValidations.add ("DiscretizedJointBound", pathValidation::createDiscretizedJointBound);
254
      pathValidations.add ("DiscretizedCollisionAndJointBound", createDiscretizedJointBoundAndCollisionChecking);
255
256
      pathValidations.add ("Progressive", continuousValidation::Progressive::create);
      pathValidations.add ("Dichotomy",   continuousValidation::Dichotomy::create);
Joseph Mirabel's avatar
Joseph Mirabel committed
257

258
259
260
261
262
263
264
265
      // 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");

266
      // Store path projector methods in map.
267
268
269
270
271
      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);
272
273
274
275
276
277
    }

    ProblemSolver::~ProblemSolver ()
    {
    }

278
279
    void ProblemSolver::distanceType (const std::string& type)
    {
280
      if (!distances.has (type)) {
281
282
283
284
    throw std::runtime_error (std::string ("No distance method with name ") +
                  type);
      }
      distanceType_ = type;
Joseph Mirabel's avatar
Joseph Mirabel committed
285
286
      // TODO
      // initDistance ();
287
288
    }

289
290
    void ProblemSolver::steeringMethodType (const std::string& type)
    {
291
      if (!steeringMethods.has (type)) {
292
293
294
295
	throw std::runtime_error (std::string ("No steering method with name ") +
				  type);
      }
      steeringMethodType_ = type;
296
      if (problem_) initSteeringMethod();
297
298
    }

299
300
    void ProblemSolver::pathPlannerType (const std::string& type)
    {
301
      if (!pathPlanners.has (type)) {
302
303
304
	throw std::runtime_error (std::string ("No path planner with name ") +
				  type);
      }
305
306
307
      pathPlannerType_ = type;
    }

308
309
    void ProblemSolver::configurationShooterType (const std::string& type)
    {
310
      if (!configurationShooters.has (type)) {
311
312
313
314
    throw std::runtime_error (std::string ("No configuration shooter with name ") +
                  type);
      }
      configurationShooterType_ = type;
315
316
      if (robot_ && problem_) {
        problem_->configurationShooter
317
          (configurationShooters.get (configurationShooterType_) (problem_));
318
      }
319
320
    }

321
    void ProblemSolver::addPathOptimizer (const std::string& type)
322
    {
323
      if (!pathOptimizers.has (type)) {
324
325
326
	throw std::runtime_error (std::string ("No path optimizer with name ") +
				  type);
      }
327
328
329
330
331
332
      pathOptimizerTypes_.push_back (type);
    }

    void ProblemSolver::clearPathOptimizers ()
    {
      pathOptimizerTypes_.clear ();
333
      pathOptimizers_.clear ();
334
335
336
337
338
339
340
341
342
343
    }

    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);
      }
344
345
    }

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

361
362
363
364
    void ProblemSolver::initPathValidation ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      PathValidationPtr_t pathValidation =
365
        pathValidations.get (pathValidationType_)
366
367
368
369
        (robot_, pathValidationTolerance_);
      problem_->pathValidation (pathValidation);
    }

370
371
372
373
    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.");
374
      problem_->clearConfigValidations();
375
376
377
378
379
380
381
382
383
384
      for (ConfigValidationTypes_t::const_iterator it =
          configValidationTypes_.begin (); it != configValidationTypes_.end ();
          ++it)
      {
        ConfigValidationPtr_t configValidation =
          configValidations.get (*it) (robot_);
        problem_->addConfigValidation (configValidation);
      }
    }

385
386
387
388
389
390
391
    void ProblemSolver::initValidations ()
    {
      initPathValidation ();
      initConfigValidation ();
      problem_->collisionObstacles(collisionObstacles_);
    }

392
393
394
    void ProblemSolver::pathProjectorType (const std::string& type,
					    const value_type& tolerance)
    {
395
      if (!pathProjectors.has (type)) {
396
397
398
399
400
401
402
	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_) {
403
	initPathProjector ();
404
405
406
      }
    }

407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
    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
422
      initConfigValidation ();
423
424
      // Set obstacles
      problem_->collisionObstacles(collisionObstacles_);
425
426
427
428
429
430
431
432
    }

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

433
434
435
436
437
438
439
440
441
442
443
444
    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)
    {
445
      RobotBuilder_t factory (robots.get (robotType_));
446
447
448
449
      assert (factory);
      return factory (name);
    }

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

    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)
    {
480
      target_ = problemTarget::GoalConfigurations::create(ProblemPtr_t());
481
482
483
484
485
486
487
488
      goalConfigurations_.push_back (config);
    }

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

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

    void ProblemSolver::resetGoalConstraint ()
    {
      goalConstraints_.reset ();
    }
540
    */
541

542
543
    void ProblemSolver::addConstraint (const ConstraintPtr_t& constraint)
    {
544
545
546
      if (robot_)
	constraints_->addConstraint (constraint);
      else
Joseph Mirabel's avatar
Joseph Mirabel committed
547
        throw std::logic_error ("Cannot add constraint while robot is not set");
548
549
550
551
    }

    void ProblemSolver::resetConstraints ()
    {
552
      if (robot_) {
553
	constraints_ = ConstraintSet::create (robot_, "Default constraint set");
554
555
556
557
        if (problem_) {
          problem_->constraints (constraints_);
        }
      }
558
559
    }

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

582
583
584
    void ProblemSolver::addLockedJointToConfigProjector
    (const std::string& configProjName, const std::string& lockedJointName)
    {
585
      addNumericalConstraintToConfigProjector (configProjName, lockedJointName);
586
587
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
588
    void ProblemSolver::comparisonType (const std::string& name,
589
        const ComparisonTypes_t types)
Joseph Mirabel's avatar
Joseph Mirabel committed
590
    {
591
      constraints::ImplicitPtr_t nc;
592
593
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
594
      else
595
596
        throw std::runtime_error (name + std::string (" is not a numerical "
              "constraint."));
597
      nc->comparisonType (types);
Joseph Mirabel's avatar
Joseph Mirabel committed
598
599
    }

600
    void ProblemSolver::comparisonType (const std::string& name,
601
        const ComparisonType &type)
602
    {
603
      constraints::ImplicitPtr_t nc;
604
605
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
606
      else
607
608
        throw std::runtime_error (name + std::string (" is not a numerical "
              "constraint."));
609
610
      ComparisonTypes_t eqtypes (nc->function().outputDerivativeSize(), type);
      nc->comparisonType (eqtypes);
611
612
    }

613
    ComparisonTypes_t ProblemSolver::comparisonType (const std::string& name) const
Joseph Mirabel's avatar
Joseph Mirabel committed
614
    {
615
      constraints::ImplicitPtr_t nc;
616
617
      if (numericalConstraints.has (name))
        nc = numericalConstraints.get(name);
618
      else
619
620
        throw std::runtime_error (name + std::string (" is not a numerical "
              "constraint."));
621
      return nc->comparisonType ();
Joseph Mirabel's avatar
Joseph Mirabel committed
622
623
    }

624
625
626
627
628
629
630
631
632
633
634
    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
635
636
      value.resize (configProjector->solver().dimension());
      size_type rows = configProjector->solver().reducedDimension();
637
      jacobian.resize (rows, configProjector->numberFreeVariables ());
638
639
640
      configProjector->computeValueAndJacobian (configuration, value, jacobian);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
641
642
643
644
645
646
647
648
649
650
651
    void ProblemSolver::maxIterProjection (size_type iterations)
    {
      maxIterProjection_ = iterations;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->maxIterations (iterations);
      }
    }

    void ProblemSolver::maxIterPathPlanning (size_type iterations)
    {
      maxIterPathPlanning_ = iterations;
652
653
      if (pathPlanner_)
        pathPlanner_->maxIterations (maxIterPathPlanning_);
Joseph Mirabel's avatar
Joseph Mirabel committed
654
655
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
656
657
658
659
660
661
662
663
    void ProblemSolver::errorThreshold (const value_type& threshold)
    {
      errorThreshold_ = threshold;
      if (constraints_ && constraints_->configProjector ()) {
        constraints_->configProjector ()->errorThreshold (threshold);
      }
    }

664
665
    void ProblemSolver::resetProblem ()
    {
666
      initializeProblem (Problem::create(robot_));
667
668
669
670
671
    }

    void ProblemSolver::initializeProblem (ProblemPtr_t problem)
    {
      problem_ = problem;
672
      resetRoadmap ();
673
674
      // Set constraints
      problem_->constraints (constraints_);
675
      // Set path validation method
676
      initPathValidation ();
677
      // Set config validation methods
678
      initConfigValidation ();
679
680
      // Set obstacles
      problem_->collisionObstacles(collisionObstacles_);
681
682
683
684
      // Distance to obstacles
      distanceBetweenObjects_ = DistanceBetweenObjectsPtr_t
	(new DistanceBetweenObjects (robot_));
      distanceBetweenObjects_->obstacles(distanceObstacles_);
685
686
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
687
688
689
690
691
    void ProblemSolver::problem (ProblemPtr_t problem)
    {
      problem_ = problem;
    }

692
693
694
695
696
697
698
    void ProblemSolver::resetRoadmap ()
    {
      if (!problem_)
        throw std::runtime_error ("The problem is not defined.");
      roadmap_ = Roadmap::create (problem_->distance (), problem_->robot());
    }

699
    void ProblemSolver::createPathOptimizers ()
700
    {
701
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
702
703
704
705
      pathOptimizers_.clear();
      for (PathOptimizerTypes_t::const_iterator it =
          pathOptimizerTypes_.begin (); it != pathOptimizerTypes_.end ();
          ++it) {
706
        PathOptimizerBuilder_t createOptimizer = pathOptimizers.get (*it);
707
        pathOptimizers_.push_back (createOptimizer (problem_));
708
709
710
      }
    }

711
712
713
    void ProblemSolver::initDistance ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
714
      DistancePtr_t dist (distances.get (distanceType_) (problem_));
715
      problem_->distance (dist);
716
717
    }

stevet's avatar
2778843    
stevet committed
718

719
    void ProblemSolver::initSteeringMethod ()
720
    {
721
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
722
      SteeringMethodPtr_t sm (
723
          steeringMethods.get (steeringMethodType_) (problem_)
724
          );
725
      problem_->steeringMethod (sm);
726
727
728
729
    }

    void ProblemSolver::initPathProjector ()
    {
730
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
731
      PathProjectorBuilder_t createProjector =
732
        pathProjectors.get (pathProjectorType_);
733
734
735
736
737
      // 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.
738
      PathProjectorPtr_t pathProjector_ =
739
        createProjector (problem_, pathProjectorTolerance_);
740
      problem_->pathProjector (pathProjector_);
741
742
    }

743
744
745
746
747
748
749
    void ProblemSolver::initProblemTarget ()
    {
      if (!problem_) throw std::runtime_error ("The problem is not defined.");
      target_->problem(problem_);
      problem_->target (target_);
    }

750
751
    void ProblemSolver::initProblem ()
    {
752
753
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

stevet's avatar
2778843    
stevet committed
754

755
756
      // Set shooter
      problem_->configurationShooter
757
        (configurationShooters.get (configurationShooterType_) (problem_));
758
759
      // Set steeringMethod
      initSteeringMethod ();
760
      PathPlannerBuilder_t createPlanner = pathPlanners.get (pathPlannerType_);
761
      pathPlanner_ = createPlanner (problem_, roadmap_);
762
      pathPlanner_->maxIterations (maxIterPathPlanning_);
763
      pathPlanner_->timeOut(timeOutPathPlanning_);
764
765
766
      roadmap_ = pathPlanner_->roadmap();
      /// create Path projector
      initPathProjector ();
767
      /// create Path optimizer
768
769
      // Reset init and goal configurations
      problem_->initConfig (initConf_);
Joseph Mirabel's avatar
Joseph Mirabel committed
770
      problem_->resetGoalConfigs ();
771
772
      for (Configurations_t::const_iterator itConfig =
	     goalConfigurations_.begin ();
Joseph Mirabel's avatar
Joseph Mirabel committed
773
	   itConfig != goalConfigurations_.end (); ++itConfig) {
Joseph Mirabel's avatar
Joseph Mirabel committed
774
	problem_->addGoalConfig (*itConfig);
775
      }
776
      initProblemTarget();
777
778
779
780
781
    }

    bool ProblemSolver::prepareSolveStepByStep ()
    {
      initProblem ();
782
783

      pathPlanner_->startSolve ();
784
      pathPlanner_->tryConnectInitAndGoals ();
785
786
787
788
789
790
791
792
793
794
795
      return roadmap_->pathExists ();
    }

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

    void ProblemSolver::finishSolveStepByStep ()
    {
796
797
      if (!roadmap_->pathExists ())
        throw std::logic_error ("No path exists.");
798
799
800
801
      PathVectorPtr_t planned =  pathPlanner_->computePath ();
      paths_.push_back (pathPlanner_->finishSolve (planned));
    }

802
803
    void ProblemSolver::solve ()
    {
804
805
      initProblem ();

806
807
      PathVectorPtr_t path = pathPlanner_->solve ();
      paths_.push_back (path);
808
      optimizePath (path);
809
810
    }

811
812
813
    bool ProblemSolver::directPath
    (ConfigurationIn_t start, ConfigurationIn_t end, bool validate,
     std::size_t& pathId, std::string& report)
814
    {
815
      report = "";
816
817
      if (!problem_) throw std::runtime_error ("The problem is not defined.");

818
819
      // Get steering method from problem
      SteeringMethodPtr_t sm (problem_->steeringMethod());
820
      PathPtr_t dp = (*sm) (start, end);
821
822
823
824
825
      if (!dp) {
	report = "Steering method failed to build a path.";
	pathId = -1;
	return false;
      }
826
      PathPtr_t dp1, dp2;
827
      PathValidationReportPtr_t r;
828
      bool projValid = true, projected = true, pathValid = true;
829
      if (validate) {
830
831
832
833
834
835
836
837
838
        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) {
839
840
841
842
843
844
          if (r) {
            std::ostringstream oss;
            oss << *r; report = oss.str ();
          } else {
            report = "No path validation report.";
          }
845
        }
846
      } else {
847
        dp2 = dp;
848
849
      }
      // Add Path in problem
850
851
852
      PathVectorPtr_t path (core::PathVector::create (dp2->outputSize (),
                            dp2->outputDerivativeSize ()));
      path->appendPath (dp2);
853
      pathId = addPath (path);
854
      return pathValid;
855
856
    }

857
    void ProblemSolver::addConfigToRoadmap (const ConfigurationPtr_t& config)
858
    {
859
      roadmap_->addNode(config);
860
    }
861

862
863
864
    void ProblemSolver::addEdgeToRoadmap (const ConfigurationPtr_t& config1,
					  const ConfigurationPtr_t& config2,
					  const PathPtr_t& path)
865
866
867
868
869
870
    {
      NodePtr_t node1, node2;
      value_type accuracy = 10e-6;
      value_type distance1, distance2;
      node1 = roadmap_->nearestNode(config1, distance1);
      node2 = roadmap_->nearestNode(config2, distance2);
871
      if (distance1 >= accuracy) {
872
	throw std::runtime_error ("No node of the roadmap contains config1");
873
      }
874
      if (distance2 >= accuracy) {
875
	throw std::runtime_error ("No node of the roadmap contains config2");
876
      }
877
      roadmap_->addEdge(node1, node2, path);
878
879
    }

880
881
882
883
884
885
886
887
888
    void ProblemSolver::interrupt ()
    {
      if (pathPlanner ()) pathPlanner ()->interrupt ();
      for (PathOptimizers_t::iterator it = pathOptimizers_.begin ();
	   it != pathOptimizers_.end (); ++it) {
	(*it)->interrupt ();
      }
    }

889
890
891
892
    void ProblemSolver::addObstacle (const DevicePtr_t& device,
				     bool collision, bool distance)
    {
      device->computeForwardKinematics();
893
      device->computeFramesForwardKinematics();
894
895
896
      device->updateGeometryPlacements();
      const std::string& prefix = device->name();

897
898
899
900
      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
901
902
903
        const ::pinocchio::Frame& frame = m.frames[i];
        // ::pinocchio::FrameType type = (frame.type == ::pinocchio::JOINT ? ::pinocchio::FIXED_JOINT : frame.type);
        obstacleRModel_->addFrame (::pinocchio::Frame (
904
905
906
907
908
909
910
911
912
              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
913
      ::pinocchio::framesForwardKinematics (*obstacleRModel_, *obstacleRData_, vector_t::Zero(0).eval());
914

915
      // Detach objects from joints
916
917
918
      for (size_type i = 0; i < device->nbObjects(); ++i) {
        CollisionObjectPtr_t obj = device->objectAt (i);
        addObstacle (prefix + obj->name (), *obj->fcl (), collision, distance);
919
920
921
      }
    }

922
    void ProblemSolver::addObstacle (const CollisionObjectPtr_t& object,
923
924
				     bool collision, bool distance)
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
925
926
927
928
929
930
931
932
933
      // 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)) {
934
935
        HPP_THROW(std::runtime_error, "object with name " << name
            << " already added! Choose another name (prefix).");
Joseph Mirabel's avatar
Joseph Mirabel committed
936
937
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
938
      ::pinocchio::GeomIndex id = obstacleModel_->addGeometryObject(::pinocchio::GeometryObject(
939
            name, 1, 0,
Joseph Mirabel's avatar
Joseph Mirabel committed
940
            inObject.collisionGeometry(),
Joseph Mirabel's avatar
Joseph Mirabel committed
941
            ::pinocchio::toPinocchioSE3(inObject.getTransform()),
942
943
            "",
            vector3_t::Ones()),
944
          *obstacleRModel_);
Joseph Mirabel's avatar
Joseph Mirabel committed
945
946
947
      // Update obstacleData_
      // FIXME This should be done in Pinocchio
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
948
949
        ::pinocchio::GeometryModel& model = *obstacleModel_;
        ::pinocchio::GeometryData& data = *obstacleData_;
Joseph Mirabel's avatar
Joseph Mirabel committed
950
951
952
953
954
955
        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(
956
              model.geometryObjects[id].geometry));
Joseph Mirabel's avatar
Joseph Mirabel committed
957
        data.oMg[id] =  model.geometryObjects[id].placement;
Joseph Mirabel's avatar
Joseph Mirabel committed
958
        data.collisionObjects[id].setTransform( ::pinocchio::toFclTransform3f(data.oMg[id]) );
Joseph Mirabel's avatar
Joseph Mirabel committed
959
960
961
      }
      CollisionObjectPtr_t object (
          new CollisionObject(obstacleModel_,obstacleData_,id));
962

963
      if (collision){