hpp-core: include/hpp/core/problem-solver.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
problem-solver.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 CNRS
3// Authors: Florent Lamiraux
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CORE_PROBLEM_SOLVER_HH
31#define HPP_CORE_PROBLEM_SOLVER_HH
32
33#include <functional>
34#include <hpp/core/config.hh>
35#include <hpp/core/container.hh>
36#include <hpp/core/deprecated.hh>
37#include <hpp/core/fwd.hh>
38#include <hpp/pinocchio/fwd.hh>
39#include <stdexcept>
40
41namespace hpp {
42namespace core {
46 class
47 Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead {
48};
49 typedef std::function<DevicePtr_t(const std::string&)> RobotBuilder_t;
50typedef std::function<PathOptimizerPtr_t(const ProblemConstPtr_t&)>
51 PathOptimizerBuilder_t;
52typedef std::function<PathPlannerPtr_t(const ProblemConstPtr_t&,
53 const RoadmapPtr_t&)>
54 PathPlannerBuilder_t;
55typedef std::function<PathValidationPtr_t(const DevicePtr_t&,
56 const value_type&)>
57 PathValidationBuilder_t;
58typedef std::function<ConfigValidationPtr_t(const DevicePtr_t&)>
59 ConfigValidationBuilder_t;
60typedef std::function<PathProjectorPtr_t(const ProblemConstPtr_t&,
61 const value_type&)>
62 PathProjectorBuilder_t;
63typedef std::function<ConfigurationShooterPtr_t(const ProblemConstPtr_t&)>
64 ConfigurationShooterBuilder_t;
65typedef std::function<DistancePtr_t(const ProblemConstPtr_t&)>
66 DistanceBuilder_t;
67typedef std::function<SteeringMethodPtr_t(const ProblemConstPtr_t&)>
68 SteeringMethodBuilder_t;
69typedef std::vector<std::pair<std::string, CollisionObjectPtr_t> >
70 AffordanceObjects_t;
71 typedef vector3_t AffordanceConfig_t;
72
78 class HPP_CORE_DLLAPI ProblemSolver {
79 public:
80 typedef std::vector<PathOptimizerPtr_t> PathOptimizers_t;
81 typedef std::vector<std::string> PathOptimizerTypes_t;
82 typedef std::vector<std::string> ConfigValidationTypes_t;
83
85 static ProblemSolverPtr_t create();
86
88 virtual ~ProblemSolver();
89
92 void robotType(const std::string& type);
93
95 const std::string& robotType() const;
96
102 DevicePtr_t createRobot(const std::string& name);
103
105 virtual void robot(const DevicePtr_t& robot);
106
108 const DevicePtr_t& robot() const;
109
111 ProblemPtr_t problem() { return problem_; }
113 const Configuration_t& initConfig() const { return initConf_; }
115 virtual void initConfig(ConfigurationIn_t config);
117 const Configurations_t& goalConfigs() const;
119 virtual void addGoalConfig(ConfigurationIn_t config);
121 void resetGoalConfigs();
123 void setGoalConstraints(const NumericalConstraints_t& constraints);
125 void resetGoalConstraints();
126 /*
128 void addGoalConstraint (const ConstraintPtr_t& constraint);
130 void addGoalConstraint (const LockedJointPtr_t& lj);
132 void addGoalConstraint (const std::string& constraintName,
133 const std::string& functionName, const std::size_t priority);
135 void resetGoalConstraint ();
136 */
138 virtual void pathPlannerType(const std::string& type);
139 const std::string& pathPlannerType() const { return pathPlannerType_; }
141 void distanceType(const std::string& type);
142 const std::string& distanceType() const { return distanceType_; }
144 void steeringMethodType(const std::string& type);
145 const std::string& steeringMethodType() const { return steeringMethodType_; }
147 void configurationShooterType(const std::string& type);
148 const std::string& configurationShooterType() const {
149 return configurationShooterType_;
150 }
152 const PathPlannerPtr_t& pathPlanner() const { return pathPlanner_; }
153
157 void addPathOptimizer(const std::string& type);
158 const PathOptimizerTypes_t& pathOptimizerTypes() const {
159 return pathOptimizerTypes_;
160 }
162 void clearPathOptimizers();
164 const PathOptimizerPtr_t& pathOptimizer(std::size_t rank) const {
165 return pathOptimizers_[rank];
166 }
167
173 void optimizePath(PathVectorPtr_t path);
174
180 virtual void pathValidationType(const std::string& type,
181 const value_type& tolerance);
182 const std::string& pathValidationType(value_type& tolerance) const {
183 tolerance = pathValidationTolerance_;
184 return pathValidationType_;
185 }
186
190 void pathProjectorType(const std::string& type, const value_type& step);
191
193 const std::string& pathProjectorType(value_type& tolerance) const {
194 tolerance = pathProjectorTolerance_;
195 return pathProjectorType_;
196 }
197
202 virtual void addConfigValidation(const std::string& type);
203
205 const ConfigValidationTypes_t configValidationTypes() {
206 return configValidationTypes_;
207 }
208
209 // Clear the vector of config validations
210 void clearConfigValidations();
211
213 void addConfigValidationBuilder(const std::string& type,
214 const ConfigValidationBuilder_t& builder);
215
216 const RoadmapPtr_t& roadmap() const { return roadmap_; }
217
220
222 void addConstraint(const ConstraintPtr_t& constraint);
223
225 const ConstraintSetPtr_t& constraints() const { return constraints_; }
226
228 virtual void resetConstraints();
229
235 virtual void addNumericalConstraintToConfigProjector(
236 const std::string& configProjName, const std::string& constraintName,
237 const std::size_t priority = 0);
238
245 void addNumericalConstraint(const std::string& name,
246 const constraints::ImplicitPtr_t& constraint) {
247 numericalConstraints.add(name, constraint);
248 }
249
252 void comparisonType(const std::string& name, const ComparisonTypes_t types);
253
254 void comparisonType(const std::string& name, const ComparisonType& type);
255
256 ComparisonTypes_t comparisonType(const std::string& name) const;
257
259 constraints::ImplicitPtr_t numericalConstraint(const std::string& name) {
260 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
261 }
262
273 void computeValueAndJacobian(const Configuration_t& configuration,
274 vector_t& value, matrix_t& jacobian) const;
275
277 void maxIterProjection(size_type iterations);
279 size_type maxIterProjection() const { return maxIterProjection_; }
280
282 void maxIterPathPlanning(size_type iterations);
284 size_type maxIterPathPlanning() const { return maxIterPathPlanning_; }
285
287 void setTimeOutPathPlanning(double timeOut) {
288 timeOutPathPlanning_ = timeOut;
289 }
290
292 double getTimeOutPathPlanning() { return timeOutPathPlanning_; }
293
295 void errorThreshold(const value_type& threshold);
297 value_type errorThreshold() const { return errorThreshold_; }
299
301 virtual void resetProblem();
302
306 virtual void resetRoadmap();
307
310
315 void createPathOptimizers();
316
320 virtual bool prepareSolveStepByStep();
321
326 virtual bool executeOneStep();
327
330 virtual void finishSolveStepByStep();
331
333 virtual void solve();
334
346 bool directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate,
347 std::size_t& pathId, std::string& report);
348
350 void addConfigToRoadmap(ConfigurationIn_t config);
351
359 void addEdgeToRoadmap(ConfigurationIn_t config1, ConfigurationIn_t config2,
360 const PathPtr_t& path);
361
363 void interrupt();
364
366 std::size_t addPath(const PathVectorPtr_t& path) {
367 std::size_t s = paths_.size();
368 paths_.push_back(path);
369 return s;
370 }
371
373 void erasePath(std::size_t pathId) {
374 PathVectors_t::iterator it = paths_.begin();
375 std::advance(it, pathId);
376
377 paths_.erase(it);
378 }
379
381 const PathVectors_t& paths() const { return paths_; }
383
386
393 virtual void addObstacle(const DevicePtr_t& device, bool collision,
394 bool distance);
395
402 virtual void addObstacle(const CollisionObjectPtr_t& inObject, bool collision,
403 bool distance);
404
407 virtual void removeObstacle(const std::string& name);
408
416 virtual void addObstacle(const std::string& name,
417 const CollisionGeometryPtr_t& inObject,
418 const Transform3s& pose, bool collision,
419 bool distance);
420
427 virtual void addObstacle(const std::string& name,
428 /*const*/ FclCollisionObject& inObject,
429 bool collision, bool distance);
430
434 void removeObstacleFromJoint(const std::string& jointName,
435 const std::string& obstacleName);
436
439 void cutObstacle(const std::string& name, const coal::AABB& aabb);
440
444 void filterCollisionPairs();
445
448 CollisionObjectPtr_t obstacle(const std::string& name) const;
449
452 const Transform3s& obstacleFramePosition(const std::string& name) const;
453
459 std::list<std::string> obstacleNames(bool collision, bool distance) const;
460
462 const DistanceBetweenObjectsPtr_t& distanceBetweenObjects() const {
463 return distanceBetweenObjects_;
464 }
465
466 pinocchio::GeomModelPtr_t obstacleGeomModel() const { return obstacleModel_; }
467 pinocchio::GeomDataPtr_t obstacleGeomData() const { return obstacleData_; }
469
471 const ObjectStdVector_t& collisionObstacles() const;
473 const ObjectStdVector_t& distanceObstacles() const;
474
476 void roadmap(const RoadmapPtr_t& roadmap) { roadmap_ = roadmap; }
477
481 void initDistance();
482
486 void initSteeringMethod();
487
491 void initPathProjector();
492
494 void initPathValidation();
495
497 void initConfigValidation();
498
500 void initValidations();
501
503 virtual void initProblemTarget();
504
507 Container<RobotBuilder_t> robots;
510 Container<ConfigurationShooterBuilder_t> configurationShooters;
513 Container<SteeringMethodBuilder_t> steeringMethods;
516 Container<DistanceBuilder_t> distances;
519 Container<PathValidationBuilder_t> pathValidations;
522 Container<ConfigValidationBuilder_t> configValidations;
525 Container<PathProjectorBuilder_t> pathProjectors;
528 Container<PathPlannerBuilder_t> pathPlanners;
531 Container<PathOptimizerBuilder_t> pathOptimizers;
532
534 Container<constraints::ImplicitPtr_t> numericalConstraints;
538 Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead
539 lockedJoints;
541 Container<CenterOfMassComputationPtr_t> centerOfMassComputations;
543 Container<segments_t> passiveDofs;
545 Container<JointAndShapes_t> jointAndShapes;
547 Container<AffordanceObjects_t> affordanceObjects;
549 Container<AffordanceConfig_t> affordanceConfigs;
550
551 protected:
555 ProblemSolver();
556
558 ConstraintSetPtr_t constraints_;
559
561 void problem(ProblemPtr_t problem);
562
568 virtual void initializeProblem(ProblemPtr_t problem);
569
571 DevicePtr_t robot_;
573 ProblemPtr_t problem_;
574
575 PathPlannerPtr_t pathPlanner_;
577 RoadmapPtr_t roadmap_;
579 PathVectors_t paths_;
581 std::string pathProjectorType_;
583 value_type pathProjectorTolerance_;
584
586 std::string pathPlannerType_;
587
589 ProblemTargetPtr_t target_;
590
591 private:
593 Configuration_t initConf_;
595 Configurations_t goalConfigurations_;
597 std::string robotType_;
599 std::string configurationShooterType_;
601 std::string distanceType_;
603 std::string steeringMethodType_;
605 PathOptimizerTypes_t pathOptimizerTypes_;
606 PathOptimizers_t pathOptimizers_;
608 std::string pathValidationType_;
610 value_type pathValidationTolerance_;
611 // Config validation methods
612 ConfigValidationTypes_t configValidationTypes_;
614 ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
615 ObjectStdVector_t distanceObstacles_; // FIXME should be removed?
616 pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
617 pinocchio::DataPtr_t obstacleRData_; // Contains the frames
618 pinocchio::GeomModelPtr_t obstacleModel_;
619 pinocchio::GeomDataPtr_t obstacleData_;
620 // Tolerance for numerical constraint resolution
621 value_type errorThreshold_;
622 // Maximal number of iterations for numerical constraint resolution
623 size_type maxIterProjection_;
625 unsigned long int maxIterPathPlanning_;
627 double timeOutPathPlanning_;
629 segmentsMap_t passiveDofsMap_;
631 CenterOfMassComputationMap_t comcMap_;
633 DistanceBetweenObjectsPtr_t distanceBetweenObjects_;
634 void initProblem();
635}; // class ProblemSolver
636} // namespace core
637} // namespace hpp
638
639#endif // HPP_CORE_PROBLEM_SOLVER_HH
hpp::core::Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead
Definition problem-solver.hh:47
hpp::core::ProblemSolver
Definition problem-solver.hh:78
hpp::core::ProblemSolver::filterCollisionPairs
void filterCollisionPairs()
hpp::core::ProblemSolver::cutObstacle
void cutObstacle(const std::string &name, const coal::AABB &aabb)
hpp::core::ProblemSolver::configValidations
Container< ConfigValidationBuilder_t > configValidations
Definition problem-solver.hh:522
hpp::core::ProblemSolver::clearPathOptimizers
void clearPathOptimizers()
Clear the vector of path optimizers.
hpp::core::ProblemSolver::resetGoalConfigs
void resetGoalConfigs()
Reset the set of goal configurations.
hpp::core::ProblemSolver::initPathValidation
void initPathValidation()
Set path validation by calling path validation factory.
hpp::core::ProblemSolver::addPathOptimizer
void addPathOptimizer(const std::string &type)
hpp::core::ProblemSolver::robotType
void robotType(const std::string &type)
hpp::core::ProblemSolver::PathOptimizers_t
std::vector< PathOptimizerPtr_t > PathOptimizers_t
Definition problem-solver.hh:80
hpp::core::ProblemSolver::executeOneStep
virtual bool executeOneStep()
hpp::core::ProblemSolver::maxIterPathPlanning
void maxIterPathPlanning(size_type iterations)
Set maximal number of iterations in config projector.
hpp::core::ProblemSolver::initSteeringMethod
void initSteeringMethod()
hpp::core::ProblemSolver::errorThreshold
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
Definition problem-solver.hh:297
hpp::core::ProblemSolver::robots
Container< RobotBuilder_t > robots
Definition problem-solver.hh:507
hpp::core::ProblemSolver::problem
void problem(ProblemPtr_t problem)
Set pointer to problem.
hpp::core::ProblemSolver::pathPlanners
Container< PathPlannerBuilder_t > pathPlanners
Definition problem-solver.hh:528
hpp::core::ProblemSolver::comparisonType
ComparisonTypes_t comparisonType(const std::string &name) const
hpp::core::ProblemSolver::removeObstacle
virtual void removeObstacle(const std::string &name)
hpp::core::ProblemSolver::pathPlannerType
virtual void pathPlannerType(const std::string &type)
Set path planner type.
hpp::core::ProblemSolver::initConfig
virtual void initConfig(ConfigurationIn_t config)
Set initial configuration.
hpp::core::ProblemSolver::addNumericalConstraint
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition problem-solver.hh:245
hpp::core::ProblemSolver::initConfig
const Configuration_t & initConfig() const
Get shared pointer to initial configuration.
Definition problem-solver.hh:113
hpp::core::ProblemSolver::getTimeOutPathPlanning
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition problem-solver.hh:292
hpp::core::ProblemSolver::ConfigValidationTypes_t
std::vector< std::string > ConfigValidationTypes_t
Definition problem-solver.hh:82
hpp::core::ProblemSolver::addObstacle
virtual void addObstacle(const CollisionObjectPtr_t &inObject, bool collision, bool distance)
hpp::core::ProblemSolver::pathOptimizer
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition problem-solver.hh:164
hpp::core::ProblemSolver::robot
const DevicePtr_t & robot() const
Get robot.
hpp::core::ProblemSolver::lockedJoints
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition problem-solver.hh:539
hpp::core::ProblemSolver::interrupt
void interrupt()
Interrupt path planning and path optimization.
hpp::core::ProblemSolver::constraints_
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition problem-solver.hh:558
hpp::core::ProblemSolver::pathProjectors
Container< PathProjectorBuilder_t > pathProjectors
Definition problem-solver.hh:525
hpp::core::ProblemSolver::pathOptimizers
Container< PathOptimizerBuilder_t > pathOptimizers
Definition problem-solver.hh:531
hpp::core::ProblemSolver::addConfigValidation
virtual void addConfigValidation(const std::string &type)
hpp::core::ProblemSolver::numericalConstraints
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition problem-solver.hh:534
hpp::core::ProblemSolver::create
static ProblemSolverPtr_t create()
Create instance and return pointer.
hpp::core::ProblemSolver::~ProblemSolver
virtual ~ProblemSolver()
Destructor.
hpp::core::ProblemSolver::configurationShooterType
const std::string & configurationShooterType() const
Definition problem-solver.hh:148
hpp::core::ProblemSolver::roadmap
const RoadmapPtr_t & roadmap() const
Definition problem-solver.hh:216
hpp::core::ProblemSolver::affordanceConfigs
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition problem-solver.hh:549
hpp::core::ProblemSolver::passiveDofs
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition problem-solver.hh:543
hpp::core::ProblemSolver::initPathProjector
void initPathProjector()
hpp::core::ProblemSolver::resetRoadmap
virtual void resetRoadmap()
hpp::core::ProblemSolver::PathOptimizerTypes_t
std::vector< std::string > PathOptimizerTypes_t
Definition problem-solver.hh:81
hpp::core::ProblemSolver::configurationShooters
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition problem-solver.hh:510
hpp::core::ProblemSolver::jointAndShapes
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition problem-solver.hh:545
hpp::core::ProblemSolver::distanceBetweenObjects
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition problem-solver.hh:462
hpp::core::ProblemSolver::directPath
bool directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate, std::size_t &pathId, std::string &report)
hpp::core::ProblemSolver::obstacle
CollisionObjectPtr_t obstacle(const std::string &name) const
hpp::core::ProblemSolver::erasePath
void erasePath(std::size_t pathId)
Erase a path.
Definition problem-solver.hh:373
hpp::core::ProblemSolver::centerOfMassComputations
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition problem-solver.hh:541
hpp::core::ProblemSolver::obstacleGeomModel
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition problem-solver.hh:466
hpp::core::ProblemSolver::steeringMethods
Container< SteeringMethodBuilder_t > steeringMethods
Definition problem-solver.hh:513
hpp::core::ProblemSolver::initDistance
void initDistance()
hpp::core::ProblemSolver::resetGoalConstraints
void resetGoalConstraints()
Stop defining the goal of path planning as a set of constraints.
hpp::core::ProblemSolver::distanceType
void distanceType(const std::string &type)
Set distance type.
hpp::core::ProblemSolver::initConfigValidation
void initConfigValidation()
Set config validation by calling config validation factories.
hpp::core::ProblemSolver::setGoalConstraints
void setGoalConstraints(const NumericalConstraints_t &constraints)
Set goal of path planning as a set of constraints.
hpp::core::ProblemSolver::addNumericalConstraintToConfigProjector
virtual void addNumericalConstraintToConfigProjector(const std::string &configProjName, const std::string &constraintName, const std::size_t priority=0)
hpp::core::ProblemSolver::pathPlanner_
PathPlannerPtr_t pathPlanner_
Definition problem-solver.hh:575
hpp::core::ProblemSolver::robot_
DevicePtr_t robot_
Robot.
Definition problem-solver.hh:571
hpp::core::ProblemSolver::maxIterProjection
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition problem-solver.hh:279
hpp::core::ProblemSolver::removeObstacleFromJoint
void removeObstacleFromJoint(const std::string &jointName, const std::string &obstacleName)
hpp::core::ProblemSolver::distances
Container< DistanceBuilder_t > distances
Definition problem-solver.hh:516
hpp::core::ProblemSolver::addObstacle
virtual void addObstacle(const std::string &name, const CollisionGeometryPtr_t &inObject, const Transform3s &pose, bool collision, bool distance)
hpp::core::ProblemSolver::robot
virtual void robot(const DevicePtr_t &robot)
Set robot.
hpp::core::ProblemSolver::addGoalConfig
virtual void addGoalConfig(ConfigurationIn_t config)
Add goal configuration.
hpp::core::ProblemSolver::prepareSolveStepByStep
virtual bool prepareSolveStepByStep()
hpp::core::ProblemSolver::obstacleGeomData
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition problem-solver.hh:467
hpp::core::ProblemSolver::pathPlannerType
const std::string & pathPlannerType() const
Definition problem-solver.hh:139
hpp::core::ProblemSolver::obstacleNames
std::list< std::string > obstacleNames(bool collision, bool distance) const
hpp::core::ProblemSolver::collisionObstacles
const ObjectStdVector_t & collisionObstacles() const
Local vector of objects considered for collision detection.
hpp::core::ProblemSolver::resetConstraints
virtual void resetConstraints()
Reset constraint set.
hpp::core::ProblemSolver::addConstraint
void addConstraint(const ConstraintPtr_t &constraint)
Add a constraint.
hpp::core::ProblemSolver::initProblemTarget
virtual void initProblemTarget()
Initialize the problem target by calling the path validation factory.
hpp::core::ProblemSolver::addConfigValidationBuilder
void addConfigValidationBuilder(const std::string &type, const ConfigValidationBuilder_t &builder)
Add a new available config validation method.
hpp::core::ProblemSolver::optimizePath
void optimizePath(PathVectorPtr_t path)
hpp::core::ProblemSolver::createRobot
DevicePtr_t createRobot(const std::string &name)
hpp::core::ProblemSolver::paths
const PathVectors_t & paths() const
Return vector of paths.
Definition problem-solver.hh:381
hpp::core::ProblemSolver::configValidationTypes
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition problem-solver.hh:205
hpp::core::ProblemSolver::finishSolveStepByStep
virtual void finishSolveStepByStep()
hpp::core::ProblemSolver::configurationShooterType
void configurationShooterType(const std::string &type)
Set configuration shooter type.
hpp::core::ProblemSolver::addConfigToRoadmap
void addConfigToRoadmap(ConfigurationIn_t config)
Add random configuration into roadmap as new node.
hpp::core::ProblemSolver::addEdgeToRoadmap
void addEdgeToRoadmap(ConfigurationIn_t config1, ConfigurationIn_t config2, const PathPtr_t &path)
hpp::core::ProblemSolver::comparisonType
void comparisonType(const std::string &name, const ComparisonTypes_t types)
hpp::core::ProblemSolver::maxIterProjection
void maxIterProjection(size_type iterations)
Set maximal number of iterations in config projector.
hpp::core::ProblemSolver::target_
ProblemTargetPtr_t target_
Shared pointer to the problem target.
Definition problem-solver.hh:589
hpp::core::ProblemSolver::distanceObstacles
const ObjectStdVector_t & distanceObstacles() const
Local vector of objects considered for distance computation.
hpp::core::ProblemSolver::setTimeOutPathPlanning
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition problem-solver.hh:287
hpp::core::ProblemSolver::clearConfigValidations
void clearConfigValidations()
hpp::core::ProblemSolver::steeringMethodType
void steeringMethodType(const std::string &type)
Set steering method type.
hpp::core::ProblemSolver::problem_
ProblemPtr_t problem_
Problem.
Definition problem-solver.hh:573
hpp::core::ProblemSolver::constraints
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition problem-solver.hh:225
hpp::core::ProblemSolver::goalConfigs
const Configurations_t & goalConfigs() const
Get number of goal configuration.
hpp::core::ProblemSolver::ProblemSolver
ProblemSolver()
hpp::core::ProblemSolver::addObstacle
virtual void addObstacle(const std::string &name, FclCollisionObject &inObject, bool collision, bool distance)
hpp::core::ProblemSolver::pathProjectorTolerance_
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition problem-solver.hh:583
hpp::core::ProblemSolver::roadmap_
RoadmapPtr_t roadmap_
Store roadmap.
Definition problem-solver.hh:577
hpp::core::ProblemSolver::addObstacle
virtual void addObstacle(const DevicePtr_t &device, bool collision, bool distance)
hpp::core::ProblemSolver::pathPlannerType_
std::string pathPlannerType_
Path planner.
Definition problem-solver.hh:586
hpp::core::ProblemSolver::pathProjectorType
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition problem-solver.hh:193
hpp::core::ProblemSolver::obstacleFramePosition
const Transform3s & obstacleFramePosition(const std::string &name) const
hpp::core::ProblemSolver::robotType
const std::string & robotType() const
Get robot type.
hpp::core::ProblemSolver::resetProblem
virtual void resetProblem()
Create new problem.
hpp::core::ProblemSolver::initValidations
void initValidations()
Initialize the config and path validations and add the obstacles.
hpp::core::ProblemSolver::solve
virtual void solve()
Set and solve the problem.
hpp::core::ProblemSolver::problem
ProblemPtr_t problem()
Get pointer to problem.
Definition problem-solver.hh:111
hpp::core::ProblemSolver::roadmap
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition problem-solver.hh:476
hpp::core::ProblemSolver::pathPlanner
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition problem-solver.hh:152
hpp::core::ProblemSolver::createPathOptimizers
void createPathOptimizers()
hpp::core::ProblemSolver::maxIterPathPlanning
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition problem-solver.hh:284
hpp::core::ProblemSolver::pathValidations
Container< PathValidationBuilder_t > pathValidations
Definition problem-solver.hh:519
hpp::core::ProblemSolver::pathValidationType
virtual void pathValidationType(const std::string &type, const value_type &tolerance)
hpp::core::ProblemSolver::pathOptimizerTypes
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition problem-solver.hh:158
hpp::core::ProblemSolver::numericalConstraint
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition problem-solver.hh:259
hpp::core::ProblemSolver::pathProjectorType_
std::string pathProjectorType_
Path projector method.
Definition problem-solver.hh:581
hpp::core::ProblemSolver::initializeProblem
virtual void initializeProblem(ProblemPtr_t problem)
hpp::core::ProblemSolver::comparisonType
void comparisonType(const std::string &name, const ComparisonType &type)
hpp::core::ProblemSolver::steeringMethodType
const std::string & steeringMethodType() const
Definition problem-solver.hh:145
hpp::core::ProblemSolver::addPath
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition problem-solver.hh:366
hpp::core::ProblemSolver::affordanceObjects
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition problem-solver.hh:547
hpp::core::ProblemSolver::distanceType
const std::string & distanceType() const
Definition problem-solver.hh:142
hpp::core::ProblemSolver::paths_
PathVectors_t paths_
Paths.
Definition problem-solver.hh:579
hpp::core::ProblemSolver::computeValueAndJacobian
void computeValueAndJacobian(const Configuration_t &configuration, vector_t &value, matrix_t &jacobian) const
hpp::core::ProblemSolver::pathProjectorType
void pathProjectorType(const std::string &type, const value_type &step)
hpp::core::ProblemSolver::pathValidationType
const std::string & pathValidationType(value_type &tolerance) const
Definition problem-solver.hh:182
hpp::core::ProblemSolver::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold in config projector.
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
container.hh
deprecated.hh
fwd.hh
hpp
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
hpp::constraints::ComparisonType
ComparisonType
hpp::core::AffordanceConfig_t
vector3_t AffordanceConfig_t
Definition problem-solver.hh:71
hpp::core::NumericalConstraints_t
constraints::NumericalConstraints_t NumericalConstraints_t
Definition fwd.hh:232
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::ObjectStdVector_t
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition fwd.hh:184
hpp::core::segmentsMap_t
std::map< std::string, segments_t > segmentsMap_t
Definition fwd.hh:231
hpp::core::PathVectorPtr_t
shared_ptr< PathVector > PathVectorPtr_t
Definition fwd.hh:193
hpp::core::PathValidationPtr_t
shared_ptr< PathValidation > PathValidationPtr_t
Definition fwd.hh:320
hpp::core::DistancePtr_t
shared_ptr< Distance > DistancePtr_t
Definition fwd.hh:141
hpp::core::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:165
hpp::core::PathVectors_t
std::vector< PathVectorPtr_t > PathVectors_t
Definition fwd.hh:215
hpp::core::PathOptimizerPtr_t
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition fwd.hh:190
hpp::core::CollisionObjectPtr_t
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition fwd.hh:99
hpp::core::Transform3s
pinocchio::Transform3s Transform3s
Definition fwd.hh:217
hpp::core::AffordanceObjects_t
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition problem-solver.hh:70
hpp::core::ConstraintPtr_t
shared_ptr< Constraint > ConstraintPtr_t
Definition fwd.hh:129
hpp::core::PathPlannerPtr_t
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition fwd.hh:191
hpp::core::PathValidationBuilder_t
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &)> PathValidationBuilder_t
Definition problem-solver.hh:57
hpp::core::ConfigurationShooterPtr_t
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition fwd.hh:113
hpp::core::RobotBuilder_t
std::function< DevicePtr_t(const std::string &)> RobotBuilder_t
Definition problem-solver.hh:49
hpp::core::PathProjectorPtr_t
shared_ptr< PathProjector > PathProjectorPtr_t
Definition fwd.hh:336
hpp::core::ProblemTargetPtr_t
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition fwd.hh:192
hpp::core::PathOptimizerBuilder_t
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &)> PathOptimizerBuilder_t
Definition problem-solver.hh:51
hpp::core::RoadmapPtr_t
shared_ptr< Roadmap > RoadmapPtr_t
Definition fwd.hh:199
hpp::core::ProblemPtr_t
shared_ptr< Problem > ProblemPtr_t
Definition fwd.hh:196
hpp::core::SteeringMethodPtr_t
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition fwd.hh:213
hpp::core::ConfigurationShooterBuilder_t
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition problem-solver.hh:64
hpp::core::PathPlannerBuilder_t
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &)> PathPlannerBuilder_t
Definition problem-solver.hh:54
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::FclCollisionObject
pinocchio::FclCollisionObject FclCollisionObject
Definition fwd.hh:102
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::CenterOfMassComputationMap_t
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition fwd.hh:234
hpp::core::DistanceBuilder_t
std::function< DistancePtr_t(const ProblemConstPtr_t &)> DistanceBuilder_t
Definition problem-solver.hh:66
hpp::core::CollisionGeometryPtr_t
pinocchio::CollisionGeometryPtr_t CollisionGeometryPtr_t
Definition fwd.hh:101
hpp::core::ConfigValidationPtr_t
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition fwd.hh:115
hpp::core::DistanceBetweenObjectsPtr_t
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition fwd.hh:142
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::ComparisonTypes_t
constraints::ComparisonTypes_t ComparisonTypes_t
Definition fwd.hh:89
hpp::core::PathProjectorBuilder_t
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &)> PathProjectorBuilder_t
Definition problem-solver.hh:62
hpp::core::ConfigValidationBuilder_t
std::function< ConfigValidationPtr_t(const DevicePtr_t &)> ConfigValidationBuilder_t
Definition problem-solver.hh:59
hpp::core::SteeringMethodBuilder_t
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &)> SteeringMethodBuilder_t
Definition problem-solver.hh:68
hpp::core::Configurations_t
std::vector< Configuration_t > Configurations_t
Definition fwd.hh:110
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:162
hpp::pinocchio::GeomModelPtr_t
shared_ptr< GeomModel > GeomModelPtr_t
hpp::pinocchio::GeomDataPtr_t
shared_ptr< GeomData > GeomDataPtr_t
hpp::pinocchio::ModelPtr_t
shared_ptr< Model > ModelPtr_t
hpp::pinocchio::DataPtr_t
shared_ptr< Data > DataPtr_t
hpp::core::Container
Definition container.hh:76