hpp::manipulation::pathPlanner::StatesPathFinder Class Reference

#include <hpp/manipulation/path-planner/states-path-finder.hh>

Inheritance diagram for hpp::manipulation::pathPlanner::StatesPathFinder:

Collaboration diagram for hpp::manipulation::pathPlanner::StatesPathFinder:

Public Types
enum	SolveStepStatus { VALID_SOLUTION , NO_SOLUTION , COLLISION_BEFORE , COLLISION_AFTER }
	Status of the step to solve for a particular waypoint. More...

Public Member Functions
virtual	~StatesPathFinder ()
StatesPathFinderPtr_t	copy () const
core::Configurations_t	computeConfigList (ConfigurationIn_t q1, ConfigurationIn_t q2)
std::vector< std::string >	constraintNamesFromSolverAtWaypoint (std::size_t wp)
std::vector< std::string >	lastBuiltTransitions () const
std::string	displayConfigsSolved () const
bool	buildOptimizationProblemFromNames (std::vector< std::string > names)
void	initWPRandom (std::size_t wp)
void	initWPNear (std::size_t wp)
void	initWP (std::size_t wp, ConfigurationIn_t q)
SolveStepStatus	solveStep (std::size_t wp)
void	reset ()
virtual void	startSolve ()
virtual void	oneStep ()
virtual void	tryConnectInitAndGoals ()

Static Public Member Functions
static StatesPathFinderPtr_t	create (const core::ProblemConstPtr_t &problem)
static StatesPathFinderPtr_t	createWithRoadmap (const core::ProblemConstPtr_t &problem, const core::RoadmapPtr_t &roadmap)

Protected Member Functions
	StatesPathFinder (const core::ProblemConstPtr_t &problem, const core::RoadmapPtr_t &)
	StatesPathFinder (const StatesPathFinder &other)
void	init (StatesPathFinderWkPtr_t weak)

Detailed Description

Optimization-based path planning method.

Sketch of the method

Given two configurations \( (q_1,q_2) \), this class formulates and solves the problem as follows.

• Compute the corresponding states \( (s_1, s_2) \).
• For each path \( (e_0, ... e_n) \) of length not more than parameter "StatesPathFinder/maxDepth" between \( (s_1, s_2)\) in the constraint graph, do:
  • define \( n-1 \) intermediate configuration \( p_i \),
  • initialize the optimization problem, as explained below,
  • solve the optimization problem, which gives \( p^*_i \),
  • in case of failure, continue the loop.

Problem formulation

Find \( (p_i) \) such that:

• \( p_0 = q_1 \),
• \( p_{n+1} = q_2 \),
• \( p_i \) is in state between \( (e_{i-1}, e_i) \), (StateFunction)
• \( (p_i, p_{i+1}) \) are reachable with transition \( e_i \) (EdgeFunction).

Problem resolution

One solver (hpp::constraints::solver::BySubstitution) is created for each waypoint \(p_i\).

• method buildOptimizationProblem builds a matrix the rows of which are the parameterizable numerical constraints present in the graph, and the columns of which are the waypoints. Each value in the matrix defines the status of each constraint right hand side for this waypoint, among {absent from the solver, equal to value for previous waypoint, equal to value for start configuration, equal to value for end configuration}.
• method analyseOptimizationProblem loops over the waypoint solvers, tests what happens when solving each waypoint after initializing only the right hand sides that are equal to the initial or goal configuration, and detects if a collision is certain to block any attempts to solve the problem in the solveOptimizationProblem step.
• method solveOptimizationProblem tries to solve for each waypoint after initializing the right hand sides with the proper values, backtracking to the previous waypoint if the solving failed or a collision is detected a number of times set from the parameter "StatesPathFinder/nTriesUntilBacktrack". If too much backtracking occurs, the method can eventually return false.
• eventually method buildPath build paths between waypoints with the constraints of the transition in which the path lies.

Current status

The method has been successfully tested with romeo holding a placard and the construction set benchmarks. The result is satisfactory except between pregrasp and grasp waypoints that may be far away from each other if the transition between those state does not contain the grasp complement constraint. The same holds between placement and pre-placement.

Member Enumeration Documentation

SolveStepStatus

enum hpp::manipulation::pathPlanner::StatesPathFinder::SolveStepStatus

Status of the step to solve for a particular waypoint.

Enumerator
VALID_SOLUTION	Valid solution (no collision)
NO_SOLUTION	Bad solve status, no solution from the solver.
COLLISION_BEFORE	Solution has collision in edge leading to the waypoint.
COLLISION_AFTER	Solution has collision in edge going from the waypoint.

Constructor & Destructor Documentation

~StatesPathFinder()

virtual hpp::manipulation::pathPlanner::StatesPathFinder::~StatesPathFinder ( )

inlinevirtual

StatesPathFinder() [1/2]

hpp::manipulation::pathPlanner::StatesPathFinder::StatesPathFinder ( const core::ProblemConstPtr_t &  problem, const core::RoadmapPtr_t &    )

protected

StatesPathFinder() [2/2]

hpp::manipulation::pathPlanner::StatesPathFinder::StatesPathFinder ( const StatesPathFinder &  other )

protected

Member Function Documentation

buildOptimizationProblemFromNames()

bool hpp::manipulation::pathPlanner::StatesPathFinder::buildOptimizationProblemFromNames

(

std::vector< std::string >

names

)

computeConfigList()

core::Configurations_t hpp::manipulation::pathPlanner::StatesPathFinder::computeConfigList	(	ConfigurationIn_t	q1,
		ConfigurationIn_t	q2
	)

create a vector of configurations between two configurations

Parameters

q1	initial configuration
q2	pointer to final configuration, NULL if goal is defined as a set of constraints

Returns

a Configurations_t from q1 to q2 if found. An empty vector if a path could not be built.

constraintNamesFromSolverAtWaypoint()

std::vector<std::string> hpp::manipulation::pathPlanner::StatesPathFinder::constraintNamesFromSolverAtWaypoint

(

std::size_t

wp

)

copy()

StatesPathFinderPtr_t hpp::manipulation::pathPlanner::StatesPathFinder::copy

(

)

const

create()

static StatesPathFinderPtr_t hpp::manipulation::pathPlanner::StatesPathFinder::create ( const core::ProblemConstPtr_t &  problem )

static

createWithRoadmap()

static StatesPathFinderPtr_t hpp::manipulation::pathPlanner::StatesPathFinder::createWithRoadmap ( const core::ProblemConstPtr_t &  problem, const core::RoadmapPtr_t &  roadmap  )

static

displayConfigsSolved()

std::string hpp::manipulation::pathPlanner::StatesPathFinder::displayConfigsSolved

(

)

const

init()

void hpp::manipulation::pathPlanner::StatesPathFinder::init ( StatesPathFinderWkPtr_t  weak )

inlineprotected

initWP()

void hpp::manipulation::pathPlanner::StatesPathFinder::initWP	(	std::size_t	wp,
		ConfigurationIn_t	q
	)

initWPNear()

void hpp::manipulation::pathPlanner::StatesPathFinder::initWPNear

(

std::size_t

wp

)

initWPRandom()

void hpp::manipulation::pathPlanner::StatesPathFinder::initWPRandom

(

std::size_t

wp

)

lastBuiltTransitions()

std::vector<std::string> hpp::manipulation::pathPlanner::StatesPathFinder::lastBuiltTransitions

(

)

const

oneStep()

virtual void hpp::manipulation::pathPlanner::StatesPathFinder::oneStep ( )

virtual

reset()

void hpp::manipulation::pathPlanner::StatesPathFinder::reset

(

)

deletes from memory the latest working states list, which is used to resume finding solutions from that list in case of failure at a later step.

solveStep()

SolveStepStatus hpp::manipulation::pathPlanner::StatesPathFinder::solveStep

(

std::size_t

wp

)

startSolve()

virtual void hpp::manipulation::pathPlanner::StatesPathFinder::startSolve ( )

virtual

tryConnectInitAndGoals()

virtual void hpp::manipulation::pathPlanner::StatesPathFinder::tryConnectInitAndGoals ( )

virtual

when both initial state is one of potential goal states, try connecting them directly

---

The documentation for this class was generated from the following file:

• include/hpp/manipulation/path-planner/states-path-finder.hh