hpp-core 4.14.0
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>
37#include <hpp/core/fwd.hh>
38#include <hpp/pinocchio/fwd.hh>
39#include <stdexcept>
40
41namespace hpp {
42namespace core {
46class
48};
49typedef std::function<DevicePtr_t(const std::string&)> RobotBuilder_t;
50typedef std::function<PathOptimizerPtr_t(const ProblemConstPtr_t&)>
52typedef std::function<PathPlannerPtr_t(const ProblemConstPtr_t&,
53 const RoadmapPtr_t&)>
55typedef std::function<PathValidationPtr_t(const DevicePtr_t&,
56 const value_type&)>
58typedef std::function<ConfigValidationPtr_t(const DevicePtr_t&)>
60typedef std::function<PathProjectorPtr_t(const ProblemConstPtr_t&,
61 const value_type&)>
63typedef std::function<ConfigurationShooterPtr_t(const ProblemConstPtr_t&)>
65typedef std::function<DistancePtr_t(const ProblemConstPtr_t&)>
67typedef std::function<SteeringMethodPtr_t(const ProblemConstPtr_t&)>
69typedef std::vector<std::pair<std::string, CollisionObjectPtr_t> >
72
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
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 ConfigurationPtr_t& initConfig() const { return initConf_; }
115 virtual void initConfig(const ConfigurationPtr_t& config);
119 virtual void addGoalConfig(const ConfigurationPtr_t& config);
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);
159 return pathOptimizerTypes_;
160 }
164 const PathOptimizerPtr_t& pathOptimizer(std::size_t rank) const {
165 return pathOptimizers_[rank];
166 }
167
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
206 return configValidationTypes_;
207 }
208
209 // Clear the vector of config validations
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
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
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
316
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
351
360 const ConfigurationPtr_t& config2,
361 const PathPtr_t& path);
362
364 void interrupt();
365
367 std::size_t addPath(const PathVectorPtr_t& path) {
368 std::size_t s = paths_.size();
369 paths_.push_back(path);
370 return s;
371 }
372
374 void erasePath(std::size_t pathId) {
375 PathVectors_t::iterator it = paths_.begin();
376 std::advance(it, pathId);
377
378 paths_.erase(it);
379 }
380
382 const PathVectors_t& paths() const { return paths_; }
384
387
394 virtual void addObstacle(const DevicePtr_t& device, bool collision,
395 bool distance);
396
403 virtual void addObstacle(const CollisionObjectPtr_t& inObject, bool collision,
404 bool distance);
405
408 virtual void removeObstacle(const std::string& name);
409
416 virtual void addObstacle(const std::string& name,
417 /*const*/ FclCollisionObject& inObject,
418 bool collision, bool distance);
419
423 void removeObstacleFromJoint(const std::string& jointName,
424 const std::string& obstacleName);
425
428 void cutObstacle(const std::string& name, const fcl::AABB& aabb);
429
434
437 CollisionObjectPtr_t obstacle(const std::string& name) const;
438
441 const Transform3f& obstacleFramePosition(const std::string& name) const;
442
448 std::list<std::string> obstacleNames(bool collision, bool distance) const;
449
452 return distanceBetweenObjects_;
453 }
454
455 pinocchio::GeomModelPtr_t obstacleGeomModel() const { return obstacleModel_; }
456 pinocchio::GeomDataPtr_t obstacleGeomData() const { return obstacleData_; }
458
463
465 void roadmap(const RoadmapPtr_t& roadmap) { roadmap_ = roadmap; }
466
471
476
481
484
487
490
492 virtual void initProblemTarget();
493
521
539
540 protected:
545
548
550 void problem(ProblemPtr_t problem);
551
557 virtual void initializeProblem(ProblemPtr_t problem);
558
563
573
575 std::string pathPlannerType_;
576
579
580 private:
582 ConfigurationPtr_t initConf_;
584 Configurations_t goalConfigurations_;
586 std::string robotType_;
588 std::string configurationShooterType_;
590 std::string distanceType_;
592 std::string steeringMethodType_;
594 PathOptimizerTypes_t pathOptimizerTypes_;
595 PathOptimizers_t pathOptimizers_;
597 std::string pathValidationType_;
599 value_type pathValidationTolerance_;
600 // Config validation methods
601 ConfigValidationTypes_t configValidationTypes_;
603 ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
604 ObjectStdVector_t distanceObstacles_; // FIXME should be removed?
605 pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
606 pinocchio::DataPtr_t obstacleRData_; // Contains the frames
607 pinocchio::GeomModelPtr_t obstacleModel_;
608 pinocchio::GeomDataPtr_t obstacleData_;
609 // Tolerance for numerical constraint resolution
610 value_type errorThreshold_;
611 // Maximal number of iterations for numerical constraint resolution
612 size_type maxIterProjection_;
614 unsigned long int maxIterPathPlanning_;
616 double timeOutPathPlanning_;
618 segmentsMap_t passiveDofsMap_;
622 DistanceBetweenObjectsPtr_t distanceBetweenObjects_;
623 void initProblem();
624}; // class ProblemSolver
625} // namespace core
626} // namespace hpp
627
628#endif // HPP_CORE_PROBLEM_SOLVER_HH
Definition: problem-solver.hh:78
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:511
void clearPathOptimizers()
Clear the vector of path optimizers.
virtual void addGoalConfig(const ConfigurationPtr_t &config)
Add goal configuration.
void resetGoalConfigs()
Reset the set of goal configurations.
void initPathValidation()
Set path validation by calling path validation factory.
void addPathOptimizer(const std::string &type)
void robotType(const std::string &type)
std::vector< PathOptimizerPtr_t > PathOptimizers_t
Definition: problem-solver.hh:80
virtual bool executeOneStep()
void maxIterPathPlanning(size_type iterations)
Set maximal number of iterations in config projector.
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
Definition: problem-solver.hh:297
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:496
void addConfigToRoadmap(const ConfigurationPtr_t &config)
Add random configuration into roadmap as new node.
void problem(ProblemPtr_t problem)
Set pointer to problem.
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:517
ComparisonTypes_t comparisonType(const std::string &name) const
virtual void removeObstacle(const std::string &name)
virtual void pathPlannerType(const std::string &type)
Set path planner type.
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition: problem-solver.hh:245
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:82
virtual void addObstacle(const CollisionObjectPtr_t &inObject, bool collision, bool distance)
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:164
const DevicePtr_t & robot() const
Get robot.
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:528
void interrupt()
Interrupt path planning and path optimization.
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:547
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:514
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:520
virtual void addConfigValidation(const std::string &type)
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:523
static ProblemSolverPtr_t create()
Create instance and return pointer.
virtual ~ProblemSolver()
Destructor.
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:216
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:538
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:532
virtual void resetRoadmap()
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:81
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:499
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:534
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:451
bool directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate, std::size_t &pathId, std::string &report)
CollisionObjectPtr_t obstacle(const std::string &name) const
void erasePath(std::size_t pathId)
Erase a path.
Definition: problem-solver.hh:374
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:530
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:455
void addEdgeToRoadmap(const ConfigurationPtr_t &config1, const ConfigurationPtr_t &config2, const PathPtr_t &path)
void cutObstacle(const std::string &name, const fcl::AABB &aabb)
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:502
virtual void initConfig(const ConfigurationPtr_t &config)
Set initial configuration.
void resetGoalConstraints()
Stop defining the goal of path planning as a set of constraints.
void distanceType(const std::string &type)
Set distance type.
void initConfigValidation()
Set config validation by calling config validation factories.
void setGoalConstraints(const NumericalConstraints_t &constraints)
Set goal of path planning as a set of constraints.
virtual void addNumericalConstraintToConfigProjector(const std::string &configProjName, const std::string &constraintName, const std::size_t priority=0)
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:564
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:560
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:279
void removeObstacleFromJoint(const std::string &jointName, const std::string &obstacleName)
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:505
virtual void robot(const DevicePtr_t &robot)
Set robot.
virtual bool prepareSolveStepByStep()
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:456
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
std::list< std::string > obstacleNames(bool collision, bool distance) const
const ObjectStdVector_t & collisionObstacles() const
Local vector of objects considered for collision detection.
virtual void resetConstraints()
Reset constraint set.
void addConstraint(const ConstraintPtr_t &constraint)
Add a constraint.
virtual void initProblemTarget()
Initialize the problem target by calling the path validation factory.
void addConfigValidationBuilder(const std::string &type, const ConfigValidationBuilder_t &builder)
Add a new available config validation method.
void optimizePath(PathVectorPtr_t path)
DevicePtr_t createRobot(const std::string &name)
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:382
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition: problem-solver.hh:205
virtual void finishSolveStepByStep()
void configurationShooterType(const std::string &type)
Set configuration shooter type.
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
void comparisonType(const std::string &name, const ComparisonTypes_t types)
void maxIterProjection(size_type iterations)
Set maximal number of iterations in config projector.
ProblemTargetPtr_t target_
Shared pointer to the problem target.
Definition: problem-solver.hh:578
const ObjectStdVector_t & distanceObstacles() const
Local vector of objects considered for distance computation.
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:287
void steeringMethodType(const std::string &type)
Set steering method type.
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:562
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem-solver.hh:225
const Configurations_t & goalConfigs() const
Get number of goal configuration.
virtual void addObstacle(const std::string &name, FclCollisionObject &inObject, bool collision, bool distance)
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:572
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:566
virtual void addObstacle(const DevicePtr_t &device, bool collision, bool distance)
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:575
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:193
const std::string & robotType() const
Get robot type.
virtual void resetProblem()
Create new problem.
void initValidations()
Initialize the config and path validations and add the obstacles.
virtual void solve()
Set and solve the problem.
ProblemPtr_t problem()
Get pointer to problem.
Definition: problem-solver.hh:111
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:465
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:152
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:284
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:508
virtual void pathValidationType(const std::string &type, const value_type &tolerance)
const Transform3f & obstacleFramePosition(const std::string &name) const
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition: problem-solver.hh:158
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition: problem-solver.hh:259
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:570
virtual void initializeProblem(ProblemPtr_t problem)
void comparisonType(const std::string &name, const ComparisonType &type)
const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:367
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:536
const std::string & distanceType() const
Definition: problem-solver.hh:142
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:568
void computeValueAndJacobian(const Configuration_t &configuration, vector_t &value, matrix_t &jacobian) const
void pathProjectorType(const std::string &type, const value_type &step)
const std::string & pathValidationType(value_type &tolerance) const
Definition: problem-solver.hh:182
void errorThreshold(const value_type &threshold)
Set error threshold in config projector.
#define HPP_CORE_DLLAPI
Definition: config.hh:64
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:231
pinocchio::value_type value_type
Definition: fwd.hh:173
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:183
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:230
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:192
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:306
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:140
pinocchio::vector3_t vector3_t
Definition: fwd.hh:164
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:214
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:189
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:98
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition: problem-solver.hh:70
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:128
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:190
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &)> PathValidationBuilder_t
Definition: problem-solver.hh:57
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:109
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:112
std::function< DevicePtr_t(const std::string &)> RobotBuilder_t
Definition: problem-solver.hh:49
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:322
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:191
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &)> PathOptimizerBuilder_t
Definition: problem-solver.hh:51
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:198
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:195
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:212
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &)> PathPlannerBuilder_t
Definition: problem-solver.hh:54
pinocchio::vector_t vector_t
Definition: fwd.hh:219
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:100
pinocchio::size_type size_type
Definition: fwd.hh:172
constraints::ComparisonType ComparisonType
Definition: fwd.hh:89
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:196
pinocchio::Transform3f Transform3f
Definition: fwd.hh:216
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:233
std::function< DistancePtr_t(const ProblemConstPtr_t &)> DistanceBuilder_t
Definition: problem-solver.hh:66
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:114
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:141
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:88
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &)> PathProjectorBuilder_t
Definition: problem-solver.hh:62
std::function< ConfigValidationPtr_t(const DevicePtr_t &)> ConfigValidationBuilder_t
Definition: problem-solver.hh:59
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &)> SteeringMethodBuilder_t
Definition: problem-solver.hh:68
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:133
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:129
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:186
pinocchio::matrix_t matrix_t
Definition: fwd.hh:161
Definition: bi-rrt-planner.hh:35
Definition: container.hh:76