Aikido
aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner Class Reference

Planner that generates a trajectory that moves the end-effector to a given pose. More...

#include <aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorPosePlanner.hpp>

Inheritance diagram for aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner:
aikido::planner::dart::ConfigurationToEndEffectorPosePlanner aikido::planner::dart::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose > aikido::planner::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose > aikido::planner::Planner

Public Member Functions

 VectorFieldConfigurationToEndEffectorPosePlanner (statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, ::dart::dynamics::MetaSkeletonPtr metaSkeleton, double goalTolerance, double anglePoseRatio, double positionTolerance, double angularTolerance, double initialStepSize, double jointLimitTolerance, double constraintCheckResolution, std::chrono::duration< double > timelimit)
 Constructor. More...
 
trajectory::TrajectoryPtr plan (const SolvableProblem &problem, Result *result=nullptr) override
 Plan to a trajectory that moves the end-effector to a given pose. More...
 
- Public Member Functions inherited from aikido::planner::dart::ConfigurationToEndEffectorPosePlanner
 ConfigurationToEndEffectorPosePlanner (statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, ::dart::dynamics::MetaSkeletonPtr metaSkeleton)
 Constructor. More...
 
virtual trajectory::TrajectoryPtr plan (const SolvableProblem &problem, Result *result=nullptr)=0
 Solves problem returning the result to result. More...
 
- Public Member Functions inherited from aikido::planner::dart::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose >
 SingleProblemPlanner (statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, ::dart::dynamics::MetaSkeletonPtr metaSkeleton)
 Constructor. More...
 
statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace () const
 Return this planner's MetaSkeletonStateSpace. More...
 
::dart::dynamics::MetaSkeletonPtr getMetaSkeleton ()
 Return this planner's MetaSkeleton. More...
 
- Public Member Functions inherited from aikido::planner::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose >
 SingleProblemPlanner (statespace::ConstStateSpacePtr stateSpace, common::RNG *rng=nullptr)
 
bool canSolve (const Problem &problem) const final override
 Returns true if this planner can solve problem. More...
 
trajectory::TrajectoryPtr plan (const Problem &problem, Result *result=nullptr) final override
 Solves problem returning the result to result. More...
 
- Public Member Functions inherited from aikido::planner::Planner
 Planner (statespace::ConstStateSpacePtr stateSpace, common::RNG *rng=nullptr)
 Constructs from a state space. More...
 
virtual ~Planner ()=default
 Default destructor. More...
 
statespace::ConstStateSpacePtr getStateSpace () const
 Returns const state space. More...
 
common::RNGgetRng ()
 Returns RNG. More...
 

Protected Attributes

double mGoalTolerance
 Tolerated geodesic distance from goal. More...
 
double mAnglePoseRatio
 Conversion ratio from radian to meter in computing the combined geodesic error. More...
 
double mPositionTolerance
 How a planned trajectory is allowed to deviated from a straight line segment defined by the direction and the distance. More...
 
double mAngularTolerance
 How a planned trajectory is allowed to deviate from a given direction. More...
 
double mInitialStepSize
 Initial step size. More...
 
double mJointLimitTolerance
 If less then this distance to joint limit, velocity is bounded in that direction to 0. More...
 
double mConstraintCheckResolution
 Resolution used in constraint checking. More...
 
std::chrono::duration< double > mTimelimit
 Timeout in seconds. More...
 
- Protected Attributes inherited from aikido::planner::dart::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose >
statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace
 Stores stateSpace pointer as a ConstMetaSkeletonStateSpacePtr. More...
 
::dart::dynamics::MetaSkeletonPtr mMetaSkeleton
 MetaSkeleton to use for planning. More...
 
- Protected Attributes inherited from aikido::planner::Planner
statespace::ConstStateSpacePtr mStateSpace
 State space associated with this planner. More...
 
std::unique_ptr< common::RNGmRng
 RNG the planner uses. More...
 

Additional Inherited Members

- Public Types inherited from aikido::planner::SingleProblemPlanner< ConfigurationToEndEffectorPosePlanner, ConfigurationToEndEffectorPose >
using SolvableProblem = ConfigurationToEndEffectorPose
 

Detailed Description

Planner that generates a trajectory that moves the end-effector to a given pose.

Constructor & Destructor Documentation

◆ VectorFieldConfigurationToEndEffectorPosePlanner()

aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::VectorFieldConfigurationToEndEffectorPosePlanner ( statespace::dart::ConstMetaSkeletonStateSpacePtr  stateSpace,
::dart::dynamics::MetaSkeletonPtr  metaSkeleton,
double  goalTolerance,
double  anglePoseRatio,
double  positionTolerance,
double  angularTolerance,
double  initialStepSize,
double  jointLimitTolerance,
double  constraintCheckResolution,
std::chrono::duration< double >  timelimit 
)

Constructor.

Parameters
[in]stateSpaceState space that this planner is associated with.
[in]metaSkeletonMetaSkeleton to plan with.
[in]goalToleranceDistance from goal to stop planning.
[in]anglePoseRatioConversion ratio from radian to meter in computing the goal tolerance.
[in]positionToleranceHow a planned trajectory is allowed to deviated from a straight line segment defined by the direction and the distance.
[in]angularToleranceHow a planned trajectory is allowed to deviate from a given direction.
[in]initialStepSizeInitial step size.
[in]jointLimitToleranceIf less then this distance to joint limit, velocity is bounded in that direction to 0.
[in]constraintCheckResolutionResolution used in constraint checking.
[in]timelimittimeout in seconds.

Member Function Documentation

◆ plan()

trajectory::TrajectoryPtr aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::plan ( const SolvableProblem problem,
Result result = nullptr 
)
override

Plan to a trajectory that moves the end-effector to a given pose.

The planner returns success if the resulting trajectory satisfies constraint at some resolution and failure (returning nullptr) otherwise. The reason for the failure is stored in the result output parameter.

Parameters
[in]problemPlanning problem.
[out]resultInformation about success or failure.
Returns
Trajectory or nullptr if planning failed.

Member Data Documentation

◆ mAnglePoseRatio

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mAnglePoseRatio
protected

Conversion ratio from radian to meter in computing the combined geodesic error.

◆ mAngularTolerance

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mAngularTolerance
protected

How a planned trajectory is allowed to deviate from a given direction.

◆ mConstraintCheckResolution

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mConstraintCheckResolution
protected

Resolution used in constraint checking.

◆ mGoalTolerance

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mGoalTolerance
protected

Tolerated geodesic distance from goal.

◆ mInitialStepSize

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mInitialStepSize
protected

Initial step size.

◆ mJointLimitTolerance

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mJointLimitTolerance
protected

If less then this distance to joint limit, velocity is bounded in that direction to 0.

◆ mPositionTolerance

double aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mPositionTolerance
protected

How a planned trajectory is allowed to deviated from a straight line segment defined by the direction and the distance.

◆ mTimelimit

std::chrono::duration<double> aikido::planner::vectorfield::VectorFieldConfigurationToEndEffectorPosePlanner::mTimelimit
protected

Timeout in seconds.