org.metacsp.multi.spatioTemporal.paths

Class TrajectoryEnvelope

java.lang.Object

org.metacsp.framework.Variable

org.metacsp.framework.multi.MultiVariable

org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope

All Implemented Interfaces:

java.io.Serializable, java.lang.Comparable<Variable>, Activity

public class TrajectoryEnvelope
extends MultiVariable
implements Activity

A TrajectoryEnvelope is a MultiVariable composed of an AllenInterval (temporal part) and two GeometricShapeVariables. One GeometricShapeVariable represents the reference path, the other represents the spatial envelope. Constraints of type AllenIntervalConstraint and DE9IMRelation can be added to TrajectoryEnvelopes.

Author:

Federico Pecora

See Also:

Serialized Form

Field Summary

Fields

Modifier and Type	Field and Description
static long	RESOLUTION

Fields inherited from class org.metacsp.framework.multi.MultiVariable

backupForSerialization, constraints, internalSolvers, variables

Fields inherited from class org.metacsp.framework.Variable

attributes, dependentVariables, id, logger, solver

Constructor Summary

Constructors

Constructor and Description
TrajectoryEnvelope(ConstraintSolver cs, int id, ConstraintSolver[] internalSolvers, Variable[] internalVars)

Method Summary

Modifier and Type	Method and Description
void	addSubEnvelope(TrajectoryEnvelope se) Add a sub-envelope to this TrajectoryEnvelope.
int	compareTo(Variable o)
static com.vividsolutions.jts.geom.Polygon	createFootprintPolygon(com.vividsolutions.jts.geom.Coordinate... coords)
protected Constraint[]	createInternalConstraints(Variable[] variables) This method must be implemented to define the internal lower-level Constraints of this MultiVariable.
static void	drawTrajectoryEnvelopeHierarchy(edu.uci.ics.jung.graph.DelegateTree<TrajectoryEnvelope,java.lang.String> tree) Draw a hierarchy of TrajectoryEnvelopes.
TrajectoryEnvelope	getClosestGroundEnvelope(long time) Get the ground TrajectoryEnvelope in which the robot will be at a given time.
double[]	getCTs() Get the start and end times of the path points representing beginning and ending of ground TrajectoryEnvelopes of this TrajectoryEnvelope.
edu.uci.ics.jung.graph.DelegateTree<TrajectoryEnvelope,java.lang.String>	getEnvelopeHierarchy()
GeometricShapeVariable	getEnvelopeVariable() Returns the spatial part of this TrajectoryEnvelope (spatial envelope).
com.vividsolutions.jts.geom.Polygon	getFootprint() Get a Polygon representing the footprint of this TrajectoryEnvelope.
static com.vividsolutions.jts.geom.Geometry	getFootprint(com.vividsolutions.jts.geom.Polygon fp, double x, double y, double theta) Returns a Geometry representing the footprint of a robot in a given pose.
TrajectoryEnvelope	getGroundEnvelope(int seqNum) Get the ground TrajectoryEnvelope in which the robot will be a the given sequence number along the trajectory.
TrajectoryEnvelope	getGroundEnvelope(long time) Get the ground TrajectoryEnvelope in which the robot will be at a given time.
java.util.TreeSet<TrajectoryEnvelope>	getGroundEnvelopes() Get the ground TrajectoryEnvelopes of this TrajectoryEnvelope, ordered by increasing start time.
java.lang.String	getInfo() Get a summary of this TrajectoryEnvelope.
com.vividsolutions.jts.geom.Polygon	getInnerFootprint() Get a Polygon representing the inner footprint of this TrajectoryEnvelope.
GeometricShapeVariable	getInternalEnvelopeVariable() Returns the spatial part of this TrajectoryEnvelope (internal spatial envelope).
double	getPartialTimeLeftEstimate(int seqNum) Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope given the current path index.
int	getPathLength() Get the length of the Trajectory underlying this TrajectoryEnvelope.
PoseSteering	getPoseSteering(long time) Get the PoseSteering along the Trajectory where the robot will be at a given time.
GeometricShapeVariable	getReferencePathVariable() Returns the spatial part of this TrajectoryEnvelope (reference path or point).
boolean	getRefinable() Assess whether this TrajectoryEnvelope can be refined into sub-envelopes.
int	getRobotID() Get the robot ID of this TrajectoryEnvelope.
int	getSequenceNumber(com.vividsolutions.jts.geom.Coordinate coord) Get the sequence number of the path point that is closest to a given coordinate.
int	getSequenceNumberEnd() Get the end sequence number of this TrajectoryEnvelope (length of trajectory -1 if this envelope has no super envelopes).
int	getSequenceNumberStart() Get the start sequence number of this TrajectoryEnvelope (0 if this envelope has no super envelopes).
java.util.ArrayList<TrajectoryEnvelope>	getSubEnvelopes() Get the sub-envelopes of this TrajectoryEnvelope.
TrajectoryEnvelope	getSuperEnvelope() Get the super-envelope of this TrajectoryEnvelope.
SymbolicVariableActivity	getSymbolicVariableActivity() Returns the symbolic part of this TrajectoryEnvelope.
java.lang.String[]	getSymbols()
AllenInterval	getTemporalVariable() Returns the temporal part of this TrajectoryEnvelope.
double	getTimeLeftEstimate(int seqNum) Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope given the current path index.
double	getTimeToEstimate(int seqNumNow, int seqNumTo) Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope between two given path indices.
Trajectory	getTrajectory() Get the Trajectory of this TrajectoryEnvelope.
Variable	getVariable()
boolean	hasSubEnvelopes() Assess whether this TrajectoryEnvelope has one or more sub-envelopes.
boolean	hasSuperEnvelope() Assess whether this TrajectoryEnvelope has a super-envelope.
com.vividsolutions.jts.geom.Geometry	makeFootprint(double x, double y, double theta) Returns a Geometry representing the footprint of the robot in a given pose.
com.vividsolutions.jts.geom.Geometry	makeFootprint(PoseSteering ps) Returns a Geometry representing the footprint of the robot in a given PoseSteering.
com.vividsolutions.jts.geom.Geometry	makeInnerFootprint(double x, double y, double theta) Returns a Geometry representing the inner footprint of the robot in a given pose.
com.vividsolutions.jts.geom.Geometry	makeInnerFootprint(PoseSteering ps) Returns a Geometry representing the inner footprint of the robot in a given PoseSteering.
void	setDomain(Domain d) Set the domain of this variable.
void	setFootprint(com.vividsolutions.jts.geom.Coordinate... coords) Set the footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope.
void	setFootprint(double w, double l, double dw, double dl) Set the footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope.
void	setFootprint(com.vividsolutions.jts.geom.Polygon footprint)
void	setInnerFootprint(com.vividsolutions.jts.geom.Coordinate... coords) Set the Inner footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope.
void	setRefinable(boolean refinable) Set whether this TrajectoryEnvelope can be refined into sub-envelopes.
void	setRobotID(int robotID) Set the robot ID of this TrajectoryEnvelope.
void	setSuperEnvelope(TrajectoryEnvelope superEnvelope) Set the super-envelope of this TrajectoryEnvelope.
void	setTrajectory(Trajectory traj) Set the Trajectory of this TrajectoryEnvelope.
java.lang.String	toString() Get a String representation of this Variable.
void	updateDuration() Imposes a temporal constraint that models the minimum duration of this TrajectoryEnvelope, derived from the minimum transition times between path poses (deltaTs).

Methods inherited from class org.metacsp.framework.multi.MultiVariable

drawVariableHierarchy, getDescription, getDomain, getInternalConstraints, getInternalConstraintSolvers, getInternalVariables, getVariableHierarchy, getVariablesFromVariableHierarchy, setComponent

Methods inherited from class org.metacsp.framework.Variable

addDependentVariables, dependsOn, equals, getAncestorVariable, getAttributes, getColor, getComponent, getConstraintSolver, getDependentVariables, getID, getMarking, getOwner, getParentVariable, getRecursivelyDependentVariables, getRootVariable, hashCode, isDependentVariable, removeDependentVariables, setAttributes, setColor, setDependentVariables, setMarking, setOwner, setParentVariable

Methods inherited from class java.lang.Object

clone, finalize, getClass, notify, notifyAll, wait, wait, wait

Field Detail

RESOLUTION

public static long RESOLUTION

Constructor Detail

TrajectoryEnvelope

public TrajectoryEnvelope(ConstraintSolver cs,
                          int id,
                          ConstraintSolver[] internalSolvers,
                          Variable[] internalVars)

Method Detail

getFootprint

public com.vividsolutions.jts.geom.Polygon getFootprint()

Get a Polygon representing the footprint of this TrajectoryEnvelope.

Returns:

A Polygon representing the footprint of this TrajectoryEnvelope.

getInnerFootprint

public com.vividsolutions.jts.geom.Polygon getInnerFootprint()

Get a Polygon representing the inner footprint of this TrajectoryEnvelope.

Returns:

A Polygon representing the footprint of this TrajectoryEnvelope.

getEnvelopeHierarchy

public edu.uci.ics.jung.graph.DelegateTree<TrajectoryEnvelope,java.lang.String> getEnvelopeHierarchy()

drawTrajectoryEnvelopeHierarchy

public static void drawTrajectoryEnvelopeHierarchy(edu.uci.ics.jung.graph.DelegateTree<TrajectoryEnvelope,java.lang.String> tree)

Draw a hierarchy of TrajectoryEnvelopes.

Parameters:

tree - The hierarchy of TrajectoryEnvelopes to draw.

getInfo

public java.lang.String getInfo()

Get a summary of this TrajectoryEnvelope.

Returns:

A String summarizing this TrajectoryEnvelope.

setFootprint

public void setFootprint(com.vividsolutions.jts.geom.Polygon footprint)

setFootprint

public void setFootprint(com.vividsolutions.jts.geom.Coordinate... coords)

Set the footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope. Provide the bounding box of the machine assuming its reference point is in (0,0).

Parameters:

coords - Coordinates of the footprint.

setInnerFootprint

public void setInnerFootprint(com.vividsolutions.jts.geom.Coordinate... coords)

Set the Inner footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope. Provide the bounding box of the machine assuming its reference point is in (0,0).

Parameters:

coords - Coordinates of the footprint.

setFootprint

public void setFootprint(double w,
                         double l,
                         double dw,
                         double dl)

Set the footprint of this TrajectoryEnvelope, which is used for computing the spatial envelope.

Parameters:

w - The width of the robot's footprint (dimension along the perpendicular to the driving direction).

l - The length of the robot's footprint (dimension along the driving direction).

dw - Lateral displacement of the reference point of the robot (along the perpendicular to the driving direction).

dl - Forward displacement of the reference point of the robot (along the driving direction).

addSubEnvelope

public void addSubEnvelope(TrajectoryEnvelope se)

Add a sub-envelope to this TrajectoryEnvelope.

Parameters:

se - A sub-envelope to add to this TrajectoryEnvelope.

hasSubEnvelopes

public boolean hasSubEnvelopes()

Assess whether this TrajectoryEnvelope has one or more sub-envelopes.

Returns:

true iff this TrajectoryEnvelope has one or more sub-envelopes.

getSubEnvelopes

public java.util.ArrayList<TrajectoryEnvelope> getSubEnvelopes()

Get the sub-envelopes of this TrajectoryEnvelope.

Returns:

The sub-envelopes of this TrajectoryEnvelope.

getTimeToEstimate

public double getTimeToEstimate(int seqNumNow,
                                int seqNumTo)

Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope between two given path indices. The estimate is based on the reference Trajectory's temporal profile.

Parameters:

seqNumNow - The path index from which the estimate should be computed.

seqNumTo - The path index to which the estimate should be computed.

Returns:

An estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope, given the current path index.

getTimeLeftEstimate

public double getTimeLeftEstimate(int seqNum)

Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope given the current path index. The estimate is based on the reference Trajectory's temporal profile.

Parameters:

seqNum - The path index from which the estimate should be computed.

Returns:

An estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope, given the current path index.

getPartialTimeLeftEstimate

public double getPartialTimeLeftEstimate(int seqNum)

Get an estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope given the current path index. The estimate is based on the reference Trajectory's temporal profile. This method returns the time left on the ground TrajectoryEnvelope on which the given path index lies.

Parameters:

seqNum - The path index from which the estimate should be computed.

Returns:

An estimate of the time left to move on the reference Trajectory of this TrajectoryEnvelope, given the current path index.

getGroundEnvelopes

public java.util.TreeSet<TrajectoryEnvelope> getGroundEnvelopes()

Get the ground TrajectoryEnvelopes of this TrajectoryEnvelope, ordered by increasing start time.

Returns:

The ground TrajectoryEnvelopes of this TrajectoryEnvelope.

getGroundEnvelope

public TrajectoryEnvelope getGroundEnvelope(int seqNum)

Get the ground TrajectoryEnvelope in which the robot will be a the given sequence number along the trajectory.

Parameters:

seqNum - The index of sequence number which the returned ground TrajectoryEnvelope should contain.

Returns:

The ground TrajectoryEnvelope at the given index in the reference Trajectory.

getGroundEnvelope

public TrajectoryEnvelope getGroundEnvelope(long time)

Get the ground TrajectoryEnvelope in which the robot will be at a given time. Returns null if the robot will not be in this TrajectoryEnvelope at the given time, or if the robot is not in any ground envelope at the given time (e.g., traveling between the end point of one ground envelope and the starting point of the next one).

Parameters:

time - The time at which to return the TrajectoryEnvelope.

Returns:

The ground TrajectoryEnvelope in which the robot will be at a given time.

getClosestGroundEnvelope

public TrajectoryEnvelope getClosestGroundEnvelope(long time)

Get the ground TrajectoryEnvelope in which the robot will be at a given time. Returns null if the robot will not be in this TrajectoryEnvelope at the given time.

Parameters:

time - The time at which to return the TrajectoryEnvelope.

Returns:

The ground TrajectoryEnvelope in which the robot will be at a given time, or the ground TrajectoryEnvelope it is leaving in case the robot is traveling between the end point of one ground envelope and the starting point of the next one.

getSuperEnvelope

public TrajectoryEnvelope getSuperEnvelope()

Get the super-envelope of this TrajectoryEnvelope.

Returns:

The super-envelope of this TrajectoryEnvelope.

getPoseSteering

public PoseSteering getPoseSteering(long time)

Get the PoseSteering along the Trajectory where the robot will be at a given time. Returns null if the robot will not be in this TrajectoryEnvelope at the given time.

Parameters:

time - The time at which to return the PoseSteering.

Returns:

The PoseSteering along the Trajectory where the robot will be at a given time.

getCTs

public double[] getCTs()

Get the start and end times of the path points representing beginning and ending of ground TrajectoryEnvelopes of this TrajectoryEnvelope.

Returns:

The start and end times of the path points representing beginning and ending of ground TrajectoryEnvelopes of this TrajectoryEnvelope.

setRobotID

public void setRobotID(int robotID)

Set the robot ID of this TrajectoryEnvelope.

Parameters:

robotID - The robot ID of this TrajectoryEnvelope.

getRobotID

public int getRobotID()

Get the robot ID of this TrajectoryEnvelope.

Returns:

The robot ID of this TrajectoryEnvelope.

setSuperEnvelope

public void setSuperEnvelope(TrajectoryEnvelope superEnvelope)

Set the super-envelope of this TrajectoryEnvelope.

Parameters:

superEnvelope - The super-envelope of this TrajectoryEnvelope.

hasSuperEnvelope

public boolean hasSuperEnvelope()

Assess whether this TrajectoryEnvelope has a super-envelope.

Returns:

true iff this TrajectoryEnvelope has a super-envelope.

setRefinable

public void setRefinable(boolean refinable)

Set whether this TrajectoryEnvelope can be refined into sub-envelopes.

Parameters:

refinable - Whether this TrajectoryEnvelope can be refined into sub-envelopes.

getRefinable

public boolean getRefinable()

Assess whether this TrajectoryEnvelope can be refined into sub-envelopes.

Returns:

true iff this TrajectoryEnvelope can be refined into sub-envelopes.

compareTo

public int compareTo(Variable o)

Specified by:

compareTo in interface java.lang.Comparable<Variable>

createInternalConstraints

protected Constraint[] createInternalConstraints(Variable[] variables)

Description copied from class: MultiVariable

This method must be implemented to define the internal lower-level Constraints of this MultiVariable.

Specified by:

createInternalConstraints in class MultiVariable

Parameters:

variables - The internal variables among which the Constraints should be defined.

Returns:

The internal lower-level Constraints of this MultiVariable.

makeFootprint

public com.vividsolutions.jts.geom.Geometry makeFootprint(PoseSteering ps)

Returns a Geometry representing the footprint of the robot in a given PoseSteering.

Parameters:

ps - The pose and steering used to create the footprint.

Returns:

A Geometry representing the footprint of the robot in a given PoseSteering.

makeInnerFootprint

public com.vividsolutions.jts.geom.Geometry makeInnerFootprint(PoseSteering ps)

Returns a Geometry representing the inner footprint of the robot in a given PoseSteering.

Parameters:

ps - The pose and steering used to create the inner footprint.

Returns:

A Geometry representing the inner footprint of the robot in a given PoseSteering.

getFootprint

public static com.vividsolutions.jts.geom.Geometry getFootprint(com.vividsolutions.jts.geom.Polygon fp,
                                                                double x,
                                                                double y,
                                                                double theta)

Returns a Geometry representing the footprint of a robot in a given pose.

Parameters:

fp - A polygon representing the footprint of the robot centered in (0,0) and appropriately oriented (can be obtained from a TrajectoryEnvelope instance via method getFootprint()).

x - The x coordinate of the pose used to create the footprint.

y - The y coordinate of the pose used to create the footprint.

theta - The orientation of the pose used to create the footprint.

Returns:

A Geometry representing the footprint of the robot in a given pose.

makeFootprint

public com.vividsolutions.jts.geom.Geometry makeFootprint(double x,
                                                          double y,
                                                          double theta)

Returns a Geometry representing the footprint of the robot in a given pose.

Parameters:

x - The x coordinate of the pose used to create the footprint.

y - The y coordinate of the pose used to create the footprint.

theta - The orientation of the pose used to create the footprint.

Returns:

A Geometry representing the footprint of the robot in a given pose.

makeInnerFootprint

public com.vividsolutions.jts.geom.Geometry makeInnerFootprint(double x,
                                                               double y,
                                                               double theta)

Returns a Geometry representing the inner footprint of the robot in a given pose.

Parameters:

x - The x coordinate of the pose used to create the inner footprint.

y - The y coordinate of the pose used to create the inner footprint.

theta - The orientation of the pose used to create the inner footprint.

Returns:

A Geometry representing the footprint of the inner robot in a given pose.

createFootprintPolygon

public static com.vividsolutions.jts.geom.Polygon createFootprintPolygon(com.vividsolutions.jts.geom.Coordinate... coords)

setTrajectory

public void setTrajectory(Trajectory traj)

Set the Trajectory of this TrajectoryEnvelope.

Parameters:

traj - The Trajectory of this TrajectoryEnvelope.

updateDuration

public void updateDuration()

Imposes a temporal constraint that models the minimum duration of this TrajectoryEnvelope, derived from the minimum transition times between path poses (deltaTs).

getSequenceNumberStart

public int getSequenceNumberStart()

Get the start sequence number of this TrajectoryEnvelope (0 if this envelope has no super envelopes).

Returns:

The starting sequence number of this TrajectoryEnvelope (0 if this envelope has no super envelopes).

getSequenceNumberEnd

public int getSequenceNumberEnd()

Get the end sequence number of this TrajectoryEnvelope (length of trajectory -1 if this envelope has no super envelopes).

Returns:

The end sequence number of this TrajectoryEnvelope (length of trajectory -1 if this envelope has no super envelopes).

getTrajectory

public Trajectory getTrajectory()

Get the Trajectory of this TrajectoryEnvelope.

Returns:

The Trajectory of this TrajectoryEnvelope.

getPathLength

public int getPathLength()

Get the length of the Trajectory underlying this TrajectoryEnvelope.

Returns:

The length of the Trajectory underlying this TrajectoryEnvelope.

setDomain

public void setDomain(Domain d)

Description copied from class: Variable

Set the domain of this variable. Must be implemented by the ConstraintSolver developer, since setting the domain of a variable may be a non-trivial operation.

Specified by:

setDomain in class Variable

Parameters:

d - The domain of the variable.

toString

public java.lang.String toString()

Description copied from class: Variable

Get a String representation of this Variable.

Specified by:

toString in class Variable

Returns:

A String representation of this Variable.

getTemporalVariable

public AllenInterval getTemporalVariable()

Returns the temporal part of this TrajectoryEnvelope.

Specified by:

getTemporalVariable in interface Activity

Returns:

An AllenInterval representing the temporal part of this TrajectoryEnvelope.

getReferencePathVariable

public GeometricShapeVariable getReferencePathVariable()

Returns the spatial part of this TrajectoryEnvelope (reference path or point). Returns a GeometricShapeVariable whose domain is either a LineStringDomain or a PointDomain.

Returns:

A GeometricShapeVariable representing the spatial part of this TrajectoryEnvelope.

getEnvelopeVariable

public GeometricShapeVariable getEnvelopeVariable()

Returns the spatial part of this TrajectoryEnvelope (spatial envelope).

Returns:

A GeometricShapeVariable representing the spatial part of this TrajectoryEnvelope.

getInternalEnvelopeVariable

public GeometricShapeVariable getInternalEnvelopeVariable()

Returns the spatial part of this TrajectoryEnvelope (internal spatial envelope).

Returns:

A GeometricShapeVariable representing the internal spatial part of this TrajectoryEnvelope.

getSymbolicVariableActivity

public SymbolicVariableActivity getSymbolicVariableActivity()

Returns the symbolic part of this TrajectoryEnvelope.

Returns:

The symbolic part of this TrajectoryEnvelope.

getSymbols

public java.lang.String[] getSymbols()

Specified by:

getSymbols in interface Activity

Returns:

A description of this Activity's symbolic variable.

getVariable

public Variable getVariable()

Specified by:

getVariable in interface Activity

getSequenceNumber

public int getSequenceNumber(com.vividsolutions.jts.geom.Coordinate coord)

Get the sequence number of the path point that is closest to a given coordinate.

Parameters:

coord - The coordinate to which the closest sequence number should be found.

Returns:

The sequence number of the path point that is closest to the given coordinate.