Configurator with long-range planning. It explores transitions out of a state until a DEFAULT Task is reached. The next state to expand will be the one with lowest heuristic cost. More...

#include <configurator.h>

Inheritance diagram for AttentiveConfigurator:

[legend]

Collaboration diagram for AttentiveConfigurator:

[legend]

Public Member Functions
	AttentiveConfigurator (Task _task)

Public Member Functions inherited from Configurator
	Configurator (Task _task)

void	init (Task _task=Task())
	Initialises configurator. More...

bool	Spawner ()
	Calls functions to explore the state space and extract a plan. More...

int	getIteration ()

void	addIteration (int i=1)

void	dummy_vertex (vertexDescriptor src)

simResult	simulate (Task, b2World &)

void	estimate_current_vertex ()

void	printPlan (std::vector< vertexDescriptor > *p=NULL)

std::pair< edgeDescriptor, bool >	addVertex (const vertexDescriptor &src, vertexDescriptor &v1, Edge edge=Edge(), bool topDown=0)
	Add state to the cognitive map and set next state Task direction and options. More...

void	start ()

void	stop ()

void	registerInterface (LIDAR_In , Motor_Out )

void	change_task ()
	changes tasks to execute on

void	update_graph (TransitionSystem &g, const b2Transform &_deltaPose)

float	approximate_angle (const float &, const Direction &, const simResult::resultType &)

void	adjust_goal_expectation ()

void	register_controller (Controller *controller)

Controller *	get_controller ()

void	register_tracker (Tracker *_tracker)

Tracker *	get_tracker () const

Motor_Out *	get_motor_interface ()

LIDAR_In *	get_lidar_interface ()

void	register_planner (Planner *_p)

void	setSimulationStep (float f)

void	register_logger (Logger *l)

Protected Member Functions
ExecutionInfo	package_info (vertexDescriptor gv=TransitionSystem::null_vertex(), bool been=false)
	Package task/goal execution details into an Execution Info instance. More...

Disturbance	getDisturbance (TransitionSystem &g, vertexDescriptor v, b2World &world, const Direction &dir, const b2Transform &start)
	Use attention window to find if any previously avoided obstacle is in the way of the goal, if present. In case of plan recycling, it shifts the disturbance to adapt to the current task ahead. More...

void	backtrack (std::vector< vertexDescriptor > &evaluation_q, std::vector< vertexDescriptor > &priority_q, std::set< vertexDescriptor > &closed, std::vector< vertexDescriptor > &plan_prov, vertexDescriptor module_src=MOVING_VERTEX, vertexDescriptor startRecycle=MOVING_VERTEX)
	Iterates through vertices, if they result in crash, it splits the tasks and recalculates evaluation functions and adds to priority queue. More...

std::vector< vertexDescriptor >	splitTask (vertexDescriptor v, Direction d, vertexDescriptor src=TransitionSystem::null_vertex())
	Split tasks into sub-states of fixed length. More...

void	propagateD (vertexDescriptor v1, vertexDescriptor v0, std::set< vertexDescriptor > *closed=NULL, StateMatcher::MATCH_TYPE match=StateMatcher::_FALSE)
	Propagate a disturbance backwards to all states representing the same task. More...

void	planPriority (TransitionSystem &, vertexDescriptor)

void	adjust_simulated_task (const vertexDescriptor &, TransitionSystem &, Task *)

void	adjust_rw_task (const vertexDescriptor &, TransitionSystem &, Task *, const b2Transform &)

std::pair< edgeDescriptor, bool >	maxProbability (std::vector< edgeDescriptor > ev, TransitionSystem &)
	Return edge with maximum probablitit in a vector. More...

VertexMatch	findMatch (State s, Direction dir=Direction::UNDEFINED, StateMatcher::MATCH_TYPE match_type=StateMatcher::_TRUE, StateDifference _sd=NULL, std::vector< VertexMatch > other_matches=NULL)
	Searches transition System for a match to a state provided. Continuous states are matched, with the option to also match the discrete state (the direction) More...

std::vector< vertexDescriptor >	explorer (vertexDescriptor v, TransitionSystem &g, b2World &w)
	Constructs transition system using a Box2D simulation combined with an A* graph expansion algorithm. More...

std::pair< bool, Direction >	getOppositeDirection (Direction)

void	resetPhi ()
	Resets all vertices evaluation function phi to a default unitialised value of 10.

std::pair< edgeDescriptor, bool >	add_vertex_now (const vertexDescriptor &src, vertexDescriptor &v1, Disturbance obs, Edge edge=Edge(), bool topDown=0)
	Adds state after discovering it in exploration. More...

std::pair< edgeDescriptor, bool >	add_vertex_retro (vertexDescriptor &src, vertexDescriptor &v1, Edge edge=Edge(), bool topDown=0)
	Adds vertices retroactively (e.g. after a state is split) More...

std::vector< Direction >	getExploredDirections (vertexDescriptor v, const std::vector< Direction > &directions)

void	removeExploredTransitions (vertexDescriptor v)

void	transitionMatrix (vertexDescriptor v, Direction d, vertexDescriptor src)

void	applyTransitionMatrix (vertexDescriptor v0, Direction d, bool ended, vertexDescriptor src, std::vector< vertexDescriptor > &plan_prov)
	Sets permitted transitions out of a state. More...

void	addToPriorityQueue (vertexDescriptor v, std::vector< vertexDescriptor > &queue, const std::set< vertexDescriptor > &closed)
	Adds vertexDescriptor to priority queue according to a custom heuristic. More...

void	ts_cleanup (TransitionSystem &, std::vector< vertexDescriptor > &)

void	shift_states (TransitionSystem &, const std::vector< vertexDescriptor > &, const b2Transform &)

vertexDescriptor	get_explore_start (TransitionSystem &)

void	pre_explore ()
	Prepares transitionsystem for exploration: clears all edges of q0 with states that aren't the current one and updates instantaneous state q0 (vertex 0) with info about the Task in execution.

void	explore_plan (b2World &) override
	Explores and plan.

std::vector< Direction >::iterator	get_next_option (vertexDescriptor v, vertexDescriptor src, std::vector< vertexDescriptor > full_plan)
	Returns an iterator to the next option representing a discrete Task transition. If v is in the plan, returns the next task in the plan. If this next task is successful, after that option has been simulated it returns iterator to vector end. If not, returns other options. More...

bool	recycle_plan (vertexDescriptor v, vertexDescriptor &v0, vertexDescriptor &task_start, StateMatcher::MATCH_TYPE &matchType, b2Transform &shift_start, b2Transform &sk_first_start, std::pair< edgeDescriptor, bool > &edge, std::vector< vertexDescriptor > &plan_prov, Direction t_get_direction)
	Assesses whether a previous plan can be recycled. More...

std::pair< State, Edge >	simulation_setup (b2World &w, Task &t, vertexDescriptor v0, b2Transform shift, b2Transform &start, std::vector< Direction >v0_options)
	Sets up for simulation. More...

void	reassign_direction (vertexDescriptor bestNext, Direction &direction)
	Reassigns direction as the direction of the bext task to expand next in explorer. More...

bool	matchToSafe (VertexMatch &match, const std::vector< VertexMatch > &other_matches=std::vector< VertexMatch >())
	if the match is a crashed task More...

std::pair< edgeDescriptor, bool >	setup_match_edge (VertexMatch &match, vertexDescriptor &v0, vertexDescriptor &v1, const Edge &k, Direction direction, bool changedMatch)
	Given a valid match, sets up the edge with the previous vertex Creates new edge if it doesn't exist, changes the match to a safe state and allow the edge to be used in planning. More...

std::vector< vertexDescriptor >	task_vertices (vertexDescriptor v, std::pair< bool, edgeDescriptor > *ep=NULL)
	Rerturns all the vertices making up a task. More...

std::vector< vertexDescriptor >	visitedOrVisitingEdge (const std::vector< edgeDescriptor > &es, TransitionSystem &g, vertexDescriptor cv=TransitionSystem::null_vertex())
	Returns a visited edge if present, or if the current. More...

vertexDescriptor	getRecyclingStart (vertexDescriptor v, vertexDescriptor v1, vertexDescriptor taskStart)
	Returns the vertex from which to start recycling plan. More...

std::vector< edgeDescriptor >	inEdges (vertexDescriptor v, Direction d=UNDEFINED)
	Returns a vector of all the in-edges of vertex. More...

bool	closeVertex (std::set< vertexDescriptor > &closed, vertexDescriptor v)

std::pair< edgeDescriptor, bool >	addEdgeRetrospectively (vertexDescriptor v, vertexDescriptor &v1, const State &s_tmp, std::pair< edgeDescriptor, bool > first_edge, Direction d, float linearSpeed)
	Adds edge retrospectively (used in split task) More...

void	correctQueue (std::vector< vertexDescriptor > &queue, vertexDescriptor v, vertexDescriptor startRecycle, int planProvSize)
	Edits. More...

void	adjustProbability (const edgeDescriptor &e)
	Adjusts the probability that a continuous state (edge target) will occur after taking a discrete state transition from the edge source. More...

Protected Attributes
StateMatcher	matcher

Protected Attributes inherited from Configurator
int	iteration =0

Task	currentTask

Controller *	task_controller =NULL

Tracker *	tracker =NULL

LIDAR_In *	ci =NULL

Motor_Out *	control =NULL

Planner *	planner =NULL

Logger *	logger =NULL

bool	running =0

std::thread *	LIDAR_thread =NULL

float	simulationStep =2*std::max(ROBOT_HALFLENGTH, ROBOT_HALFWIDTH)

std::chrono::high_resolution_clock::time_point	previousTimeScan

GoalChanger *	goal_changer =NULL

std::vector< vertexDescriptor >	m_plan

std::vector< vertexDescriptor >	current_vertices

int	bodies =0

Task	controlGoal

CoordinateContainer	data2fp

TransitionSystem	transitionSystem =TransitionSystem(1)

WorldBuilder	worldBuilder

vertexDescriptor	currentVertex =MOVING_VERTEX

edgeDescriptor	movingEdge =edgeDescriptor()

edgeDescriptor	currentEdge =edgeDescriptor()

Additional Inherited Members
Static Public Member Functions inherited from Configurator
static void	run (Configurator *)
	Spawns tasks, creates plans, tracks task and controls real-world task switching option to run in thread. Thread can be used if planning time might exceed 200ms (LIDAR sampling rate) but doesn't have to be.

static void	MulT (const b2Transform &B, Task &task)
	Matrix multiply by transpose.

static void	Mul (const b2Transform &B, Task &task)
	Matrix multiplication.

Detailed Description

Configurator with long-range planning. It explores transitions out of a state until a DEFAULT Task is reached. The next state to expand will be the one with lowest heuristic cost.

Member Function Documentation

◆ add_vertex_now()

std::pair< edgeDescriptor, bool > AttentiveConfigurator::add_vertex_now	(	const vertexDescriptor &	src,
		vertexDescriptor &	v1,
		Disturbance	obs,
		Edge	edge = `Edge()`,
		bool	topDown = `0`
	)

protected

Adds state after discovering it in exploration.

Parameters

src	source state
v1	new state
Di	the initial disturbance of v1
edge	connecting edge between src->v1
topDown	flag determining whether state v1 has been simulated already or not

◆ add_vertex_retro()

std::pair< edgeDescriptor, bool > AttentiveConfigurator::add_vertex_retro	(	vertexDescriptor &	src,
		vertexDescriptor &	v1,
		Edge	edge = `Edge()`,
		bool	topDown = `0`
	)

protected

Adds vertices retroactively (e.g. after a state is split)

Parameters

src	source state
v1	new state
edge	connecting edge between src->v1
topDown	flag determining whether state v1 has been simulated already or not

◆ addEdgeRetrospectively()

std::pair< edgeDescriptor, bool > AttentiveConfigurator::addEdgeRetrospectively	(	vertexDescriptor	v,
		vertexDescriptor &	v1,
		const State &	s_tmp,
		std::pair< edgeDescriptor, bool >	first_edge,
		Direction	d,
		float	linearSpeed
	)

protected

Adds edge retrospectively (used in split task)

Parameters

v	source
v1	target
s_tmp_endPose	endPose for the new sub-state
first_edge	original unsplit task edge
Direction	d

Returns: std::pair<edgeDescriptor, bool>

◆ addToPriorityQueue()

void AttentiveConfigurator::addToPriorityQueue	(	vertexDescriptor	v,
		std::vector< vertexDescriptor > &	queue,
		const std::set< vertexDescriptor > &	closed
	)

protected

Adds vertexDescriptor to priority queue according to a custom heuristic.

Parameters

v	the vertex descriptor for the state
queue	the priority queue
g	the transition system
closed	closed states: ones which have already been simulated and expanded

◆ adjustProbability()

void AttentiveConfigurator::adjustProbability ( const edgeDescriptor & e )

protected

Adjusts the probability that a continuous state (edge target) will occur after taking a discrete state transition from the edge source.

Parameters

e	an edge descriptor

◆ applyTransitionMatrix()

void AttentiveConfigurator::applyTransitionMatrix	(	vertexDescriptor	v0,
		Direction	d,
		bool	ended,
		vertexDescriptor	src,
		std::vector< vertexDescriptor > &	plan_prov
	)

protected

Sets permitted transitions out of a state.

Parameters

v0	vertex descriptor for source state
d	direction of state (to be removed later)
ended	whether the overarching goal has ended
src	source vertex of v0
plan_prov	the plan

◆ backtrack()

void AttentiveConfigurator::backtrack	(	std::vector< vertexDescriptor > &	evaluation_q,
		std::vector< vertexDescriptor > &	priority_q,
		std::set< vertexDescriptor > &	closed,
		std::vector< vertexDescriptor > &	plan_prov,
		vertexDescriptor	module_src = `MOVING_VERTEX`,
		vertexDescriptor	startRecycle = `MOVING_VERTEX`
	)

protected

Iterates through vertices, if they result in crash, it splits the tasks and recalculates evaluation functions and adds to priority queue.

Parameters

evaluation_q	evaluation queue: lists of vertices to be evaluated for splitting
priority_q	the priority queue to add vertices to
closed	closed set
plan_prov	provisional plan
module_src	source vertex from which modular expansion began
startRecycle	vertex from which plan recycling started (for correcting pq)

◆ correctQueue()

void AttentiveConfigurator::correctQueue	(	std::vector< vertexDescriptor > &	queue,
		vertexDescriptor	v,
		vertexDescriptor	startRecycle,
		int	planProvSize
	)

protected

Edits.

Parameters

v	out of the queue if the plan was recycled from a different vertex. Useful if the frontier of
v	results in a crash and a plan needs to be recycled from the state previous to it
queue	queue
v	source v (which may have a crash in its frontier)
startRecycle	another state found to precede the frontier
planProvSize	size of the provisional plan: indicates if the recycling was successful or not

◆ explorer()

std::vector< vertexDescriptor > AttentiveConfigurator::explorer	(	vertexDescriptor	v,
		TransitionSystem &	g,
		b2World &	w
	)

protected

Constructs transition system using a Box2D simulation combined with an A* graph expansion algorithm.

Parameters

v	starting vertex
g	the transition system
w	box2d world

Returns: std::vector<vertexDescriptor> a plan, if recycled from previous knowledge

◆ findMatch()

VertexMatch AttentiveConfigurator::findMatch	(	State	s,
		Direction	dir = `Direction::UNDEFINED`,
		StateMatcher::MATCH_TYPE	match_type = `StateMatcher::_TRUE`,
		StateDifference *	_sd = `NULL`,
		std::vector< VertexMatch > *	other_matches = `NULL`
	)

protected

Searches transition System for a match to a state provided. Continuous states are matched, with the option to also match the discrete state (the direction)

Parameters

s	the state to find a match for
dir	simulated task direction (allowing to match continuous states only)
match_type	fuzzy operator indicating what parameters in the continuous state match
_sd	pointer to state difference, can be used for further calculatins
other_matches	pointer to a vector of other matches (all of the type defined by
match_type)

Returns: VertexMatch the best match found and its type

◆ get_next_option()

std::vector<Direction>::iterator AttentiveConfigurator::get_next_option	(	vertexDescriptor	v,
		vertexDescriptor	src,
		std::vector< vertexDescriptor >	full_plan
	)

protected

Returns an iterator to the next option representing a discrete Task transition. If v is in the plan, returns the next task in the plan. If this next task is successful, after that option has been simulated it returns iterator to vector end. If not, returns other options.

Parameters

v	vertex being expanded
src	v's source vertex
full_plan	a plan

Returns: std::vector<Direction>::iterator

◆ getDisturbance()

Disturbance AttentiveConfigurator::getDisturbance	(	TransitionSystem &	g,
		vertexDescriptor	v,
		b2World &	world,
		const Direction &	dir,
		const b2Transform &	start
	)

protected

Use attention window to find if any previously avoided obstacle is in the way of the goal, if present. In case of plan recycling, it shifts the disturbance to adapt to the current task ahead.

Parameters

g	the transition system
v	source vertex
world	box2d world
dir	direction of the task to be simulated
start	task start

Returns: previous Di if it's in the way of target, even though source task was successful previous Dn if previous task is safe for now goal Di otherwise

◆ getExploredDirections()

std::vector< Direction > AttentiveConfigurator::getExploredDirections	(	vertexDescriptor	v,
		const std::vector< Direction > &	directions
	)

protected

returns a vector of which directions in vector
Parameters

directions were explored (at the present iteration) from vertex

v

◆ getOppositeDirection()

std::pair< bool, Direction > AttentiveConfigurator::getOppositeDirection ( Direction d )

protected

Returns: std::pair <bool, Direction>(opposite exists, opposite direction)

◆ getRecyclingStart()

vertexDescriptor AttentiveConfigurator::getRecyclingStart	(	vertexDescriptor	v,
		vertexDescriptor	v1,
		vertexDescriptor	taskStart
	)

protected

Returns the vertex from which to start recycling plan.

Parameters

v	source vertex which is being expanded
v1	last vertex in task, or the match
taskStart	start of the task

Returns

Parameters

v	if the task is successful,
connectingEdge	if it ends in crash

◆ inEdges()

std::vector< edgeDescriptor > AttentiveConfigurator::inEdges	(	vertexDescriptor	v,
		Direction	d = `UNDEFINED`
	)

protected

Returns a vector of all the in-edges of vertex.

Parameters

v.	Option to enter
d	to select a subset of edge. Does not return self-edges

Returns: std::vector <edgeDescriptor>

◆ matchToSafe()

bool AttentiveConfigurator::matchToSafe	(	VertexMatch &	match,
		const std::vector< VertexMatch > &	other_matches = `std::vector<VertexMatch>()`
	)

protected

if the match is a crashed task

Parameters

match
other_matches

Returns: true if changes match

◆ maxProbability()

std::pair< edgeDescriptor, bool > AttentiveConfigurator::maxProbability	(	std::vector< edgeDescriptor >	ev,
		TransitionSystem &	g
	)

protected

Return edge with maximum probablitit in a vector.

Parameters

ev	vector of edges

Returns: std::pair <edgeDescriptor, bool>

◆ package_info()

ExecutionInfo AttentiveConfigurator::package_info	(	vertexDescriptor	gv = `TransitionSystem::null_vertex()`,
		bool	been = `false`
	)

inlineprotected

Package task/goal execution details into an Execution Info instance.

Parameters

gv	goal vertex
been	has goal been visited

Returns: Planner::ExecutionInfo

◆ propagateD()

void AttentiveConfigurator::propagateD	(	vertexDescriptor	v1,
		vertexDescriptor	v0,
		std::set< vertexDescriptor > *	closed = `NULL`,
		StateMatcher::MATCH_TYPE	match = `StateMatcher::_FALSE`
	)

protected

Propagate a disturbance backwards to all states representing the same task.

Parameters

v1	final vertex linked to the final sub-state in the task
v0	source of v1
closed	closed set
match	is v1 a match of any kind to a vertex in the graph

◆ reassign_direction()

void AttentiveConfigurator::reassign_direction	(	vertexDescriptor	bestNext,
		Direction &	direction
	)

protected

Reassigns direction as the direction of the bext task to expand next in explorer.

Parameters

bestNext	vertices representing task with lowest phi
direction	direction to reassign

◆ recycle_plan()

bool AttentiveConfigurator::recycle_plan	(	vertexDescriptor	v,
		vertexDescriptor &	v0,
		vertexDescriptor &	task_start,
		StateMatcher::MATCH_TYPE &	matchType,
		b2Transform &	shift_start,
		b2Transform &	sk_first_start,
		std::pair< edgeDescriptor, bool > &	edge,
		std::vector< vertexDescriptor > &	plan_prov,
		Direction	t_get_direction
	)

protected

Assesses whether a previous plan can be recycled.

Parameters

v	source vertex (start of module)
v0	vertex currently expanded
task_start	vertex representing the start of the provisional plan
matchType	match type of matching operation
shift_start	vector to shift future start by
sk_first_start	start of the state just simulated (sk.first)
edge	edge between v0 and the new state (v1)
plan_prov	provisional plan
t_get_direction	direction of the task just simulated

Returns: true; false

◆ removeExploredTransitions()

void AttentiveConfigurator::removeExploredTransitions ( vertexDescriptor v )

protected

only keeps unexplored transitions out of vertex
Parameters

v

◆ setup_match_edge()

std::pair< edgeDescriptor, bool > AttentiveConfigurator::setup_match_edge	(	VertexMatch &	match,
		vertexDescriptor &	v0,
		vertexDescriptor &	v1,
		const Edge &	k,
		Direction	direction,
		bool	changedMatch
	)

protected

Given a valid match, sets up the edge with the previous vertex Creates new edge if it doesn't exist, changes the match to a safe state and allow the edge to be used in planning.

Parameters

match
v0
v1
k
direction

Returns: std::pair<edgeDescriptor, bool>

◆ simulation_setup()

std::pair< State, Edge > AttentiveConfigurator::simulation_setup	(	b2World &	w,
		Task &	t,
		vertexDescriptor	v0,
		b2Transform	shift,
		b2Transform &	start,
		std::vector< Direction >	v0_options
	)

protected

Sets up for simulation.

Parameters

W	box2d world
t	task (gets modified)
v0	source vertex for the next state
shift	any shift to be applied (in case of plan recycling)
start	task start
v0_options	a subset of transitionSystem[0].options

Returns: sk, pair of state and edge

◆ splitTask()

std::vector< vertexDescriptor > AttentiveConfigurator::splitTask	(	vertexDescriptor	v,
		Direction	d,
		vertexDescriptor	src = `TransitionSystem::null_vertex()`
	)

protected

Split tasks into sub-states of fixed length.

Parameters

v	the vertex to split
d	direction of the task to split
src	source vertex for v

Returns: std::vector <vertexDescriptor> : the vertices making up substates in the original task

◆ task_vertices()

std::vector< vertexDescriptor > AttentiveConfigurator::task_vertices	(	vertexDescriptor	v,
		std::pair< bool, edgeDescriptor > *	ep = `NULL`
	)

protected

Rerturns all the vertices making up a task.

Parameters

v	a vertex representing a state
ep	connecting edge to the task

◆ transitionMatrix()

void AttentiveConfigurator::transitionMatrix	(	vertexDescriptor	v,
		Direction	d,
		vertexDescriptor	src
	)

protected

Combines edges K and jump function: represents possible transitions out of a state

Parameters

v	the vertex to which transitions are being assigned
d	state direction (redundant)
src	source vertex of state

◆ visitedOrVisitingEdge()

std::vector<vertexDescriptor> AttentiveConfigurator::visitedOrVisitingEdge	(	const std::vector< edgeDescriptor > &	es,
		TransitionSystem &	g,
		vertexDescriptor	cv = `TransitionSystem::null_vertex()`
	)

protected

Returns a visited edge if present, or if the current.

Parameters

es
g
cv

Returns: std::vector <vertexDescriptor>

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

src/configurator.h
src/configurator.cpp

directions	were explored (at the present iteration) from vertex
v

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ add_vertex_now()

◆ add_vertex_retro()

◆ addEdgeRetrospectively()

◆ addToPriorityQueue()

◆ adjustProbability()

◆ applyTransitionMatrix()

◆ backtrack()

◆ correctQueue()

◆ explorer()

◆ findMatch()

◆ get_next_option()

◆ getDisturbance()

◆ getExploredDirections()

◆ getOppositeDirection()

◆ getRecyclingStart()

◆ inEdges()

◆ matchToSafe()

◆ maxProbability()

◆ package_info()

◆ propagateD()

◆ reassign_direction()

◆ recycle_plan()

◆ removeExploredTransitions()

◆ setup_match_edge()

◆ simulation_setup()

◆ splitTask()

◆ task_vertices()

◆ transitionMatrix()

◆ visitedOrVisitingEdge()