A planner that minimizes the the displacement cost of violated constraints using a RRT-like strategy. More...

#include <DisplacementPlanner.h>

Classes
struct	DynamicShortestPathNode

struct	Edge

struct	Milestone

struct	PathSearchNode

struct	TestResult
	stores all values for a single obstacle displacement test More...

Public Types
typedef std::vector< TestResult >	TestResults

typedef Graph::UndirectedGraph< Milestone, Edge >	Roadmap

Public Member Functions
	DisplacementPlanner (ObstacleDisplacementCSpace *space)

void	Init (const Config &start, const Config &goal)

bool	Plan (int numIters, int numExpandsPerDisp, int numLocalOptimize, Real expandLimitStep, std::vector< int > &bestPath, std::vector< int > &bestDisplacements)

void	Expand (Real maxTotalCost, std::vector< int > &newNodes)
	Performs one iteration of planning given a limit on the solution cost.

int	AddNewDisplacement (Real maxTotalCost=Inf)

virtual std::pair< int, Real >	PickObstacleToSample (Real maxTotalCost)

virtual std::pair< int, Real >	PickExploreObstacle (Real maxTotalCost)

virtual std::pair< int, Real >	PickGoalExploreObstacle (Real maxTotalCost)

virtual std::pair< int, Real >	PickRefineObstacle (Real maxTotalCost)

virtual std::pair< int, Real >	PickGoalRefineObstacle (Real maxTotalCost)

bool	IsCandidateForExploration (int i) const
	Returns true if this node is an unexplored candidate.

void	BuildRoadmap (Real maxExplanationCost, RoadmapPlanner &prm)
	Outputs the graph with the given explanation limit.

double	Cost (const std::vector< int > &assignment) const

int	AddNode (const Config &q, int parent=-1)

void	AddEdge (int i, int j)

void	AddEdge (int i, int j, const TestResults &tests)

int	CheckImmovableAndAddNode (const Config &q, int parent=-1)

int	ExtendEdge (int i, const Config &q)

void	KNN (const Config &q, int k, std::vector< int > &neighbors, std::vector< Real > &distances)

void	KNN (const Config &q, Real maxTotalCost, int k, std::vector< int > &neighbors, std::vector< Real > &distances)

bool	UpdateCoversIn (int nstart, Real maxTotalCost)

void	UpdateCoversOut (int nstart, Real maxTotalCost)

std::shared_ptr< PathSearchNode >	UpdateEdge (int i, int j, PathSearchNode *ni, Real maxTotalCost)

bool	CheckUpstreamConstraints (PathSearchNode *n, int constraint, int sample)

void	PropagateDownstream (PathSearchNode *n, std::vector< std::shared_ptr< PathSearchNode > > &nodes, Real maxTotalCost)

bool	FindMinimumAssignment (PathSearchNode *n, Real maxTotalCost)

bool	CheckNodeConstraint (Milestone &v, int constraint, int sample)

bool	CheckEdgeConstraint (Edge &e, int constraint, int sample)

Real	OptimalCost (int i) const

PathSearchNode *	OptimalPathTo (int i)

void	PruneSearchNode (PathSearchNode n, IndexedPriorityQueue< PathSearchNode , Real > *q=NULL)

bool	Revisited (PathSearchNode *n)

bool	Dominates (PathSearchNode a, PathSearchNode b)

bool	ParetoDominates (const std::vector< int > &a, const std::vector< int > &b) const

void	LocalImprovementCandidates (int i, std::vector< int > &obstacles)

bool	GenerateDisplacementSample (int obstacle, Real maxDispCost, Real maxTotalCost, int numTries)

bool	AddDisplacementSample (int obstacle, const Vector &disp)

void	AddDisplacementSampleRaw (int obstacle, const Vector &disp)
	Adds a new displacement sample, without checking or updating paths.

bool	RefineGoalDisplacements (int numIters, Real perturbRadiusFrac=1.0)

bool	RefineGoalPathAndDisplacements (int numIters, Real perturbRadiusFrac=1.0, Real lipschitzDisp=1.0)

bool	ShortcutGoalPath (int skip, int numIters)

void	GetPath (PathSearchNode *n, std::vector< int > &path) const
	Returns the path from the start leading to this node.

void	GetMilestonePath (const std::vector< int > &path, MilestonePath &mpath) const
	Returns the MilestonePath corresponding to a graph path.

void	GetMilestonePath (PathSearchNode *n, MilestonePath &mpath) const

bool	SanityCheck (bool checkOneCoverGreedy=false)
	Do basic sanity checks with the data structures.

Public Attributes
Config	start

Config	goal

ObstacleDisplacementCSpace *	space

std::vector< Real >	initialDisplacementCosts

Real	pathCostWeight

bool	dynamicDomainExpansion

int	numConnections

Real	connectThreshold

Real	expandDistance

Real	goalConnectThreshold

Real	goalBiasProbability

bool	bidirectional

bool	updatePathsComplete

int	updatePathsMax
	For complete planning, keep at most this number of covers.

int	obstacleSampleCount

int	obstacleDescendIters

Roadmap	roadmap

std::vector< std::vector< Vector > >	displacementSamples

std::vector< std::vector< Real > >	displacementSampleCosts

std::vector< std::vector< int > >	displacementSampleOrders

std::vector< DynamicShortestPathNode >	pathCovers

int	numExpands

int	numRefinementAttempts

int	numRefinementSuccesses

int	numExplorationAttempts

int	numEdgeChecks

int	numConfigChecks

int	numUpdateCovers

int	numUpdateCoversIterations

double	timeNearestNeighbors

double	timeRefine

double	timeExplore

double	timeUpdateCoversIn

double	timeUpdateCoversOut

double	timeOverhead

Detailed Description

A planner that minimizes the the displacement cost of violated constraints using a RRT-like strategy.

Usage: //first, set up an ObstacleDisplacementCSpace cspace. DisplacementPlanner planner(&cspace); planner.Init(start,goal);

//do planning with a given expansion schedule vector<Real> limits; vector<int> numIters; limits.push_back(limit1); numIters.push_back(100); ... limits.push_back(limitN); numIters.push_back(100); vector<int> assignment; if(planner.Plan(limits,numIters,bestPlan,assignment)) LOG4CXX_INFO(KrisLibrary::logger(),"Success\n");

Plan will keep going until all iterations are exhausted. It will use the given limit until the goal is reached, then it will use the existing cost to goal as the limit in the future.

More detailed manual control (e.g., breaking on first path) is possible through calling:

Expand to grow the roadmap
AddNewDisplacement to generate a new displacement sample
Test whether the goal is reached by calling bool success = (OptimalPathTo(1)!=NULL);
GetMilestonePath(OptimalPathTo(1),path) to extract the path to the goal

Member Function Documentation

bool DisplacementPlanner::AddDisplacementSample	(	int	obstacle,
		const Vector &	disp
	)

Adds a new displacement sample, if it could possibly affect the optimal path to any node. Returns false if it is irrelevant

References Math::IsInf().

int DisplacementPlanner::AddNewDisplacement ( Real maxTotalCost = Inf )

Picks an obstacle to sample and samples a displacement, returns -1 if failed

bool DisplacementPlanner::Dominates	(	PathSearchNode *	a,
		PathSearchNode *	b
	)

returns true if a dominates b, either in the total cost sense (greedy) or pareto sense (optimal)

bool DisplacementPlanner::GenerateDisplacementSample	(	int	obstacle,
		Real	maxDispCost,
		Real	maxTotalCost,
		int	numTries
	)

Generates a new displacement sample that has the potential to improve paths to nodes that currently collide with the given obstacle. The displacement with lowest cost is picked out of the best of numTries attempts

References Math::IsInf().

void DisplacementPlanner::GetMilestonePath	(	PathSearchNode *	n,
		MilestonePath &	mpath
	)		const

Returns the MilestonePath leading up to a given PathSearchNode e.g., GetMilestonePath(OptimalPathTo(1),path) returns the optimal path to the goal.

void DisplacementPlanner::LocalImprovementCandidates	(	int	i,
		std::vector< int > &	obstacles
	)

For a given graph node, returns the list of obstacles that would improve the optimal path to that node if it were removed.

pair< int, Real > DisplacementPlanner::PickExploreObstacle ( Real maxTotalCost )

virtual

The explore strategy finds a node that is not expanded and finds the obstacles that might enable the node to be expanded

The explore strategy finds a node that is not reached and finds the obstacles that might make the node reached.

pair< int, Real > DisplacementPlanner::PickGoalExploreObstacle ( Real maxTotalCost )

virtual

The goal explore strategy looks for candidate edges into the goal and picks among obstacles overlapping those edges

pair< int, Real > DisplacementPlanner::PickGoalRefineObstacle ( Real maxTotalCost )

virtual

The refine strategy picks among obstacles that must be moved along the current path to the goal

pair< int, Real > DisplacementPlanner::PickObstacleToSample ( Real maxTotalCost )

virtual

Picks an obstacle to sample, and a maximum cost bound for the displacement, based on the current roadmap. Default implementation picks whether to explore or refine current paths.

References Math::Rand(), and Math::RandInt().

pair< int, Real > DisplacementPlanner::PickRefineObstacle ( Real maxTotalCost )

virtual

The refine strategy picks among obstacles that must be moved along current paths (over the whole roadmap)

bool DisplacementPlanner::Plan	(	int	numIters,
		int	numExpandsPerDisp,
		int	numLocalOptimize,
		Real	expandLimitStep,
		std::vector< int > &	bestPath,
		std::vector< int > &	bestDisplacements
	)

Performs bottom-up planning according to a given set of parameters (see publication)

bool DisplacementPlanner::RefineGoalDisplacements	(	int	numIters,
		Real	perturbRadiusFrac = `1.0`
	)

Performs local optimization of the displacements along the goal path. Sample-based descent technique that at each step perturbs the optimum by r*perturbRadiusFrac where r is the current deviation from the zero displacement. Returns true if a better solution is found.

bool DisplacementPlanner::RefineGoalPathAndDisplacements	(	int	numIters,
		Real	perturbRadiusFrac = `1.0`,
		Real	lipschitzDisp = `1.0`
	)

Performs local optimization of the displacements and the goal path. Sample-based descent technique that at each step perturbs the optimum displacement by r*perturbRadiusFrac where r is the current deviation from the zero displacement, and then modifies the path to achieve that displacement. Paths are perturbed by radius lipschitzDisp*r*perturbRadiusFrac, where lipschitzDisp is a lipschitz constant on how much a C-obstacle moves with a unit displacement. Returns true if a better solution is found.

bool DisplacementPlanner::ShortcutGoalPath	(	int	skip,
		int	numIters
	)

Performs local optimization of the path to the goal via shortcutting. This version considers skip-node shortcutting

Member Data Documentation

bool DisplacementPlanner::updatePathsComplete

If true: use the slower complete, exact cover update. If false: use the faster greedy one.

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

DisplacementPlanner.h
DisplacementPlanner.cpp

Classes

Public Types

Public Member Functions

Public Attributes

Detailed Description

Member Function Documentation

Member Data Documentation