sk.fiit.jim.agent.trajectory

Class TrajectoryPlanner

java.lang.Object

sk.fiit.jim.agent.trajectory.TrajectoryPlanner

public class TrajectoryPlanner
extends Object

TrajectoryPlanner.java This class calculates which moves must agent make in order to get from given start position and rotation to desired end position and rotation. These moves are ordered in queue which represents the trajectory. Basic idea of algorithm: 1. agent rotates so he is facingthe final position 2. agents walks to the final position 3. agents rotates again to achieve the final rotation There can be also obstacles in agents trajectory. In order to bypass obstacles agent calculates around these obstacles. Trajectory is calculated with different deviations in order to calculate fastest trajectory possible.

Field Summary

Fields

Modifier and Type	Field and Description
(package private) double	actualPhi Actual rotation of agent during trajectory calculations.
(package private) Vector3D	actualPos Actual position of agent during trajectory calculations.
static double	ANGLE_DEVIATION_A
static double	ANGLE_DEVIATION_B
static double	ANGLE_DEVIATION_C
static double	ANGLE_DEVIATION_D
static double	ANGLE_DEVIATION_E
static double	DISTANCE_DEVIATION
(package private) Vector3D	finalPosition Final position of agent.
(package private) double	finalRotation Final rotation of agent.
(package private) Annotation	lastMove Stores the last move in the trajectory for purpose of compatibility testing of the next move.
(package private) LinkedList<Annotation>	moves Queue of moves used to store partial trajectories calculated by rotate() and walk() methods.
(package private) LinkedList<Vector3D>	obstacles Contains all possible obstcles on trajectory.
(package private) ArrayList<Annotation>	rotation Contains all moves tagged as rotation.
(package private) Vector3D	startPosition Starting position of agent.
(package private) double	startRotation Starting rotation of agent.
(package private) Annotation	stdWalk Annotation of the move that is standard for walking.
(package private) Trajectory	trajectory Represents fastest calculated trajectory.
(package private) LinkedList<Trajectory>	trajectoryPlans Contains all calculated trajectories.
(package private) ArrayList<Annotation>	walk Contains all moves tagged as walk.

Constructor Summary

Constructors

Constructor and Description
TrajectoryPlanner(Vector3D startPosition, double startRotation, Vector3D finalPosition, double finalRotation, LinkedList<Vector3D> obstacles) Constructs the object of this class and calls all methods needed to compute the optimal trajectory.

Method Summary

Methods

Modifier and Type	Method and Description
private Vector3D	findBypassPoint(Vector3D position, double rotation, Vector3D obstacle) Finds bypass point of given obstacle considering also other obstacles.
double	getDistance() Sums the walk length of all moves of operational trajectory returning distance to by walked.
Trajectory	getTrajectory() Returns the fastest calculated trajectory.
Trajectory	pickBestTrajectory() Sums the move durations of all trajectories and returns the one with shortest duration.
private void	plan(Trajectory t) Plans the trajectory using rotations and walk moves, bypassing obstacles.
private double	rotate(double angle, double deviation) Adjusts the angle and calls rotateQueue() method.
private double	rotateQueue(double angle, ArrayList<Annotation> list, double deviation) Calculates the trajectory of rotating.
private void	setFinal(Vector3D position, double rotation) Sets final position and rotation of the agent.
private void	setStart(Vector3D position, double rotation) Sets start position and rotation of the agent.
private void	walk(double distance) Calculates the trajectory of walking.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

trajectory

Trajectory trajectory

Represents fastest calculated trajectory.

rotation

ArrayList<Annotation> rotation

Contains all moves tagged as rotation.

walk

ArrayList<Annotation> walk

Contains all moves tagged as walk.

obstacles

LinkedList<Vector3D> obstacles

Contains all possible obstcles on trajectory.

trajectoryPlans

LinkedList<Trajectory> trajectoryPlans

Contains all calculated trajectories.

moves

LinkedList<Annotation> moves

Queue of moves used to store partial trajectories calculated by rotate() and walk() methods.

startPosition

Vector3D startPosition

Starting position of agent.

startRotation

double startRotation

Starting rotation of agent.

finalPosition

Vector3D finalPosition

Final position of agent.

finalRotation

double finalRotation

Final rotation of agent.

actualPos

Vector3D actualPos

Actual position of agent during trajectory calculations.

actualPhi

double actualPhi

Actual rotation of agent during trajectory calculations.

ANGLE_DEVIATION_A

public static final double ANGLE_DEVIATION_A

See Also:

Constant Field Values

ANGLE_DEVIATION_B

public static final double ANGLE_DEVIATION_B

See Also:

Constant Field Values

ANGLE_DEVIATION_C

public static final double ANGLE_DEVIATION_C

See Also:

Constant Field Values

ANGLE_DEVIATION_D

public static final double ANGLE_DEVIATION_D

See Also:

Constant Field Values

ANGLE_DEVIATION_E

public static final double ANGLE_DEVIATION_E

See Also:

Constant Field Values

DISTANCE_DEVIATION

public static final double DISTANCE_DEVIATION

See Also:

Constant Field Values

stdWalk

Annotation stdWalk

Annotation of the move that is standard for walking. Should be straight with minimal y-axis deviation, stable and fast.

lastMove

Annotation lastMove

Stores the last move in the trajectory for purpose of compatibility testing of the next move.

Constructor Detail

TrajectoryPlanner

public TrajectoryPlanner(Vector3D startPosition,
                 double startRotation,
                 Vector3D finalPosition,
                 double finalRotation,
                 LinkedList<Vector3D> obstacles)

Constructs the object of this class and calls all methods needed to compute the optimal trajectory.

Parameters:

startPosition -

startRotation -

finalPosition -

finalRotation -

obstacles -

Method Detail

setStart

private void setStart(Vector3D position,
            double rotation)

Sets start position and rotation of the agent.

Parameters:

position -

rotation -

setFinal

private void setFinal(Vector3D position,
            double rotation)

Sets final position and rotation of the agent.

Parameters:

position -

rotation -

plan

private void plan(Trajectory t)

Plans the trajectory using rotations and walk moves, bypassing obstacles. Deviation is determined in given trajectory as parameter.

Parameters:

trajectory - to fill with moves

rotate

private double rotate(double angle,
            double deviation)

Adjusts the angle and calls rotateQueue() method.

Parameters:

angle - to rotate

acceptable - deviation

Returns:

angle deviation

walk

private void walk(double distance)

Calculates the trajectory of walking. Creates queue of 'walk' moves.

Parameters:

distance -

rotateQueue

private double rotateQueue(double angle,
                 ArrayList<Annotation> list,
                 double deviation)

Calculates the trajectory of rotating. Creates queue of 'rotate' moves.

Parameters:

distance -

pickBestTrajectory

public Trajectory pickBestTrajectory()

Sums the move durations of all trajectories and returns the one with shortest duration.

Returns:

trajectory

getTrajectory

public Trajectory getTrajectory()

Returns the fastest calculated trajectory.

Returns:

trajectory

getDistance

public double getDistance()

Sums the walk length of all moves of operational trajectory returning distance to by walked.

Returns:

distance of subtrajectory

findBypassPoint

private Vector3D findBypassPoint(Vector3D position,
                       double rotation,
                       Vector3D obstacle)

Finds bypass point of given obstacle considering also other obstacles.

Parameters:

position - of agent

rotation - of agent facing obstacle

obstacle -

Returns: