Configurator that discretises DEFAULT tasks into fixed-length segments. Basically, this is an implementation of classic A* search. More...

#include <attentive.h>

Inheritance diagram for DiscreteConfigurator:

[legend]

Collaboration diagram for DiscreteConfigurator:

[legend]

Public Member Functions
Disturbance	getDisturbance (TransitionSystem &g, vertexDescriptor v, b2World &world, const Direction &dir, const b2Transform &start) override
	In discrete configurator, Di is previous state's Dn, or the goal, if null.

Robot	makeRobot (b2World &w, const b2Transform &start) override
	Makes the robot object in the world with the given start position and task. More...

float	remainingSimulationTime (const Task *const t=NULL) override
	Sets a time limit to DEFAULT tasks corresponding to the amount of time estimated to complete a forward move of length simulationStep.

VertexMatch	findMatch (State s, Direction dir=Direction::UNDEFINED, StateMatcher::MATCH_TYPE match_type=StateMatcher::_TRUE, StateDifference _sd=NULL, std::vector< VertexMatch > other_matches=NULL) override
	In discrete configurator, matches must be exact and no signal to replan if Task is successful. More...

StateMatcher::MATCH_TYPE	desiredMatch () override

void	transitionMatrix (vertexDescriptor v, Direction d, vertexDescriptor src) override
	In discrete configurator, successful DEFAULT tasks can transition to DEFAULT, LEFT, RIGHT. More...

bool	closeVertex (std::set< vertexDescriptor > &closed, vertexDescriptor v) override
	Just adds to closed set.

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) override
	Does not recycle.

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

std::vector< Direction >	partiallyExplorativeOptions (std::pair< bool, edgeDescriptor > ve) override
	Assigns options to vertex which has in previous iteration been expanded. Assigns direction of successful current task, or direction of other tasks if unsuccessful. More...

bool	shouldPartiallyExplore (const std::vector< edgeDescriptor > &oe, std::pair< bool, edgeDescriptor > ve) override
	Psi guard: should only current task be simulated?

void	removeExploredTransitions (vertexDescriptor v) override
	If the current vertex is matched, don't allow to check plan further. More...

bool	canPropagate (vertexDescriptor v) override
	propagates only if the previous vertex More...

bool	canReassignOutcome (vertexDescriptor v) override

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

float	customSimulationStep (vertexDescriptor v=TransitionSystem::null_vertex())

Public Member Functions inherited from AttentiveConfigurator
	AttentiveConfigurator (Task _task)

Public Member Functions inherited from Configurator
	Configurator (Task _task)

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

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

int	getIteration ()

void	addIteration (int i=1)

void	assignDisturbanceToTask (const Disturbance &d, Task &t)

void	dummy_vertex (vertexDescriptor src)

simResult	simulate (Task t, b2World &w)
	Simulates this task, includes creating the robot object. More...

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 )

virtual void	change_task ()
	changes tasks executing on the robot

void	update_graph (TransitionSystem &g, const TrackingResult &tr)

float	approximate_angle (float angle, Direction d, simResult::resultType outcome)
	Approximating angle to divisors of pi/2. More...

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

void	adjust_simulated_task (const vertexDescriptor &v, Task &t)
	If Task. More...

void	register_controller (Controller *controller)

Controller *	get_controller ()

void	register_tracker (Tracker *_tracker)
	registers and initialises the tracker to the goal

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)

void	register_goalChanger (GoalChanger *gc)

bool	areInterfacesSetUp (Configurator *c=NULL)

void	assignBodyFeatures (Task &t, const BodyFeatures &bf)
	Assigns body features to the disturbance of a task (NOTE: affordance and validity of the disturbance will remain the same) More...

void	assignDimensions (Task &t, float halfLength, float halfWidth)
	Assigns dimensions to the disturbance of a task (NOTE: pose, affordance and validity of the disturbance will remain the same) More...

void	register_worldBuilder (WorldBuilder *wb)

b2Transform	get_start (const Task *const t)

Direction	get_direction (const Task *const t)

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	InvMul (const b2Transform &B, Task &task)
	Matrix multiply by transpose.

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

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

Protected Member Functions inherited from FocusedConfigurator
void	adjust_goal_expectation () override
	Uses tracking information to adjust the position of the goal relative to the robot.

bool	hasPlanFinished ()

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

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

void	planPriority (TransitionSystem &, vertexDescriptor)

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

virtual 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...

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	applyTransitionMatrix (vertexDescriptor v0, Direction d, bool ended, vertexDescriptor src, std::vector< vertexDescriptor > &plan_prov)
	Sets permitted transitions out of a state, formally combines jump guard J and guard Psi. 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 ()

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...

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...

virtual std::vector< vertexDescriptor >	task_vertices (vertexDescriptor v, std::pair< bool, edgeDescriptor > *ep=NULL)
	Rerturns all the vertices making up a task. 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...

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...

void	abandonPlan (std::vector< vertexDescriptor > &planProv, vertexDescriptor v0, vertexDescriptor v1)
	Clears plan vector, sets current task to change and clears current vertices. More...

void	visitedDirectionsPushBack (vertexDescriptor v, std::vector< Direction > &visitedDirections, Direction direction)
	Pushes directions visited at this iteration (not using phi) to input vector. More...

void	enforce_edge ()
	Used to make sure that there is always a connection between DUMMY/MOVING to the next in plan - useful in sudden changes of plans.

Protected Attributes inherited from AttentiveConfigurator
StateMatcher	matcher

Protected Attributes inherited from Configurator
int	iteration =0

int	simulatedTasks =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

Task	controlGoal

CoordinateContainer	data2fp

TransitionSystem	transitionSystem =TransitionSystem(1)

WorldBuilder *	worldBuilder =new WorldBuilder()

vertexDescriptor	currentVertex =MOVING_VERTEX

edgeDescriptor	movingEdge =edgeDescriptor()

edgeDescriptor	currentEdge =movingEdge

Detailed Description

Configurator that discretises DEFAULT tasks into fixed-length segments. Basically, this is an implementation of classic A* search.

Member Function Documentation

◆ backtrack()

void DiscreteConfigurator::backtrack	(	std::vector< vertexDescriptor > &	evaluation_q,
		std::vector< vertexDescriptor > &	priority_q,
		std::set< vertexDescriptor > &	closed,
		std::vector< vertexDescriptor > &	plan_prov,
		vertexDescriptor	module_src,
		vertexDescriptor	startRecycle
	)

overridevirtual

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)

Reimplemented from AttentiveConfigurator.

◆ canPropagate()

bool DiscreteConfigurator::canPropagate ( vertexDescriptor v )

overridevirtual

propagates only if the previous vertex

Parameters

v	is a dummy vertex, but can be customised

Reimplemented from AttentiveConfigurator.

◆ findMatch()

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

overridevirtual

In discrete configurator, matches must be exact and no signal to replan if Task is successful.

Parameters

s	state to match
dir	direction of the task
match_type	type of desired match
_sd	pointer to state difference
other_matches	pointer to other matches

Returns: VertexMatch

Reimplemented from FocusedConfigurator.

◆ makeRobot()

Robot DiscreteConfigurator::makeRobot	(	b2World &	world,
		const b2Transform &	start
	)

overridevirtual

Makes the robot object in the world with the given start position and task.

Parameters

world
start
taskWithGoal

Returns: Robot

Reimplemented from AttentiveConfigurator.

◆ partiallyExplorativeOptions()

std::vector< Direction > DiscreteConfigurator::partiallyExplorativeOptions ( std::pair< bool, edgeDescriptor > ve )

overridevirtual

Assigns options to vertex which has in previous iteration been expanded. Assigns direction of successful current task, or direction of other tasks if unsuccessful.

Parameters

ve	a visited edge and whetehr it exists

Reimplemented from AttentiveConfigurator.

◆ propagateD()

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

overridevirtual

Propagate a disturbance backwards to all states representing the same task (max 1)

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

Returns: whether a criteria to continue propagating is met

Reimplemented from AttentiveConfigurator.

◆ removeExploredTransitions()

void DiscreteConfigurator::removeExploredTransitions ( vertexDescriptor v )

overridevirtual

If the current vertex is matched, don't allow to check plan further.

Parameters

v

Reimplemented from FocusedConfigurator.

◆ transitionMatrix()

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

overridevirtual

In discrete configurator, successful DEFAULT tasks can transition to DEFAULT, LEFT, RIGHT.

Parameters

v	vertex of source state
d	direction of the Task
src	source vertex of v

Reimplemented from AttentiveConfigurator.

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

src/attentive.h
src/attentive.cpp

Public Member Functions

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ backtrack()

◆ canPropagate()

◆ findMatch()

◆ makeRobot()

◆ partiallyExplorativeOptions()

◆ propagateD()

◆ removeExploredTransitions()

◆ transitionMatrix()