Class TimedElasticBand

Defined in File timed_elastic_band.h

Class Documentation

class TimedElasticBand

Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.

All trajectory related methods (initialization, modifying, …) are implemented inside this class.

Let

\( Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \)

be a sequence of poses,

in which

\( \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \)

denotes a single pose of the robot.

The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between two consecutive poses, resuting in a sequence of

n-1 time intervals \( \Delta T_i \): \( \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \)

.

Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration. The tuple of both sequences defines the underlying trajectory.

Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.

Utility and status methods

PoseSequence pose_vec_: Internal container storing the sequence of optimzable pose vertices.

TimeDiffSequence timediff_vec_: Internal container storing the sequence of optimzable timediff vertices.

int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d> &ref_point, double *distance = nullptr, int begin_idx = 0) const

Find the closest point on the trajectory w.r.t. to a provided reference point.

This function can be useful to find the part of a trajectory that is close to an obstacle.

Parameters:

ref_point – reference point (2D position vector)
distance – [out] [optional] the resulting minimum distance
begin_idx – start search at this pose index

Returns:

Index to the closest pose in the pose sequence

int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d> &ref_line_start, const Eigen::Ref<const Eigen::Vector2d> &ref_line_end, double *distance = nullptr) const

Find the closest point on the trajectory w.r.t. to a provided reference line.

This function can be useful to find the part of a trajectory that is close to an (line) obstacle.

Parameters:

ref_line_start – start of the reference line (2D position vector)
ref_line_end – end of the reference line (2D position vector)
distance – [out] [optional] the resulting minimum distance

Returns:

Index to the closest pose in the pose sequence

int findClosestTrajectoryPose(const Point2dContainer &vertices, double *distance = nullptr) const

Find the closest point on the trajectory w.r.t. to a provided reference polygon.

This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.

Parameters:

vertices – vertex container containing Eigen::Vector2d points (the last and first point are connected)
distance – [out] [optional] the resulting minimum distance

Returns:

Index to the closest pose in the pose sequence

int findClosestTrajectoryPose(const Obstacle &obstacle, double *distance = nullptr) const

Find the closest point on the trajectory w.r.t to a provided obstacle type.

This function can be useful to find the part of a trajectory that is close to an obstacle. The method is calculates appropriate distance metrics for point, line and polygon obstacles. For all unknown obstacles the centroid is used.

Parameters:

obstacle – Subclass of the Obstacle base class
distance – [out] [optional] the resulting minimum distance

Returns:

Index to the closest pose in the pose sequence

inline int sizePoses() const: Get the length of the internal pose sequence.

inline int sizeTimeDiffs() const: Get the length of the internal timediff sequence.

inline bool isInit() const: Check whether the trajectory is initialized (nonzero pose and timediff sequences)

double getSumOfAllTimeDiffs() const: Calculate the total transition time (sum over all time intervals of the timediff sequence)

double getSumOfTimeDiffsUpToIdx(int index) const

Calculate the estimated transition time up to the pose denoted by index.

Parameters:: index – Index of the pose up to which the transition times are summed up
Returns:: Estimated transition time up to pose index

double getAccumulatedDistance() const: Calculate the length (accumulated euclidean distance) of the trajectory.

bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot = -1, int skip_poses = 0)

Check if all trajectory points are contained in a specific region.

The specific region is a circle around the current robot position (Pose(0)) with given radius radius. This method investigates a different radius for points behind the robot if max_dist_behind_robot >= 0.

Parameters:

radius – radius of the region with the robot position (Pose(0)) as center
max_dist_behind_robot – A separate radius for trajectory points behind the robot, activated if 0 or positive
skip_poses – If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), … are tested.

Returns:

true, if all tested trajectory points are inside the specified region, false otherwise.

Access pose and timediff sequences

inline PoseSequence &poses()

Access the complete pose sequence.

Returns:: reference to the pose sequence

inline const PoseSequence &poses() const

Access the complete pose sequence (read-only)

Returns:: const reference to the pose sequence

inline TimeDiffSequence &timediffs()

Access the complete timediff sequence.

Returns:: reference to the dimediff sequence

inline const TimeDiffSequence &timediffs() const

Access the complete timediff sequence.

Returns:: reference to the dimediff sequence

inline double &TimeDiff(int index)

Access the time difference at pos index of the time sequence.

Parameters:: index – element position inside the internal TimeDiffSequence
Returns:: reference to the time difference at pos index

inline const double &TimeDiff(int index) const

Access the time difference at pos index of the time sequence (read-only)

Parameters:: index – element position inside the internal TimeDiffSequence
Returns:: const reference to the time difference at pos index

inline PoseSE2 &Pose(int index)

Access the pose at pos index of the pose sequence.

Parameters:: index – element position inside the internal PoseSequence
Returns:: reference to the pose at pos index

inline const PoseSE2 &Pose(int index) const

Access the pose at pos index of the pose sequence (read-only)

Parameters:: index – element position inside the internal PoseSequence
Returns:: const reference to the pose at pos index

inline PoseSE2 &BackPose(): Access the last PoseSE2 in the pose sequence.

inline const PoseSE2 &BackPose() const: Access the last PoseSE2 in the pose sequence (read-only)

inline double &BackTimeDiff(): Access the last TimeDiff in the time diff sequence.

inline const double &BackTimeDiff() const: Access the last TimeDiff in the time diff sequence (read-only)

inline VertexPose *PoseVertex(int index)

Access the vertex of a pose at pos index for optimization purposes.

Parameters:: index – element position inside the internal PoseSequence
Returns:: Weak raw pointer to the pose vertex at pos index

inline VertexTimeDiff *TimeDiffVertex(int index)

Access the vertex of a time difference at pos index for optimization purposes.

Parameters:: index – element position inside the internal TimeDiffSequence
Returns:: Weak raw pointer to the timediff vertex at pos index

Append new elements to the pose and timediff sequences

void addPose(const PoseSE2 &pose, bool fixed = false)

Append a new pose vertex to the back of the pose sequence.

Parameters:

pose – PoseSE2 to push back on the internal PoseSequence
fixed – Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

void addPose(const Eigen::Ref<const Eigen::Vector2d> &position, double theta, bool fixed = false)

Append a new pose vertex to the back of the pose sequence.

Parameters:

position – 2D vector representing the position part
theta – yaw angle representing the orientation part
fixed – Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

void addPose(double x, double y, double theta, bool fixed = false)

Append a new pose vertex to the back of the pose sequence.

Parameters:

x – x-coordinate of the position part
y – y-coordinate of the position part
theta – yaw angle representing the orientation part
fixed – Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

void addTimeDiff(double dt, bool fixed = false)

Append a new time difference vertex to the back of the time diff sequence.

Parameters:

dt – time difference value to push back on the internal TimeDiffSequence
fixed – Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

void addPoseAndTimeDiff(const PoseSE2 &pose, double dt)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Warning

Since the timediff is defined to connect two consecutive poses, this call is only allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,…): therefore add a single pose using addPose() first!

Parameters:

pose – PoseSE2 to push back on the internal PoseSequence
dt – time difference value to push back on the internal TimeDiffSequence

void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d> &position, double theta, double dt)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Warning

see addPoseAndTimeDiff(const PoseSE2& pose, double dt)

Parameters:

position – 2D vector representing the position part
theta – yaw angle representing the orientation part
dt – time difference value to push back on the internal TimeDiffSequence

void addPoseAndTimeDiff(double x, double y, double theta, double dt)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Warning

see addPoseAndTimeDiff(const PoseSE2& pose, double dt)

Parameters:

x – x-coordinate of the position part
y – y-coordinate of the position part
theta – yaw angle representing the orientation part
dt – time difference value to push back on the internal TimeDiffSequence

Insert new elements and remove elements of the pose and timediff sequences

void insertPose(int index, const PoseSE2 &pose)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters:

index – element position inside the internal PoseSequence
pose – PoseSE2 element to insert into the internal PoseSequence

void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d> &position, double theta)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters:

index – element position inside the internal PoseSequence
position – 2D vector representing the position part
theta – yaw-angle representing the orientation part

void insertPose(int index, double x, double y, double theta)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters:

index – element position inside the internal PoseSequence
x – x-coordinate of the position part
y – y-coordinate of the position part
theta – yaw-angle representing the orientation part

void insertTimeDiff(int index, double dt)

Insert a new timediff vertex at pos. index to the timediff sequence.

Parameters:

index – element position inside the internal TimeDiffSequence
dt – timediff value

void deletePose(int index)

Delete pose at pos. index in the pose sequence.

Parameters:: index – element position inside the internal PoseSequence

void deletePoses(int index, int number)

Delete multiple (number) poses starting at pos. index in the pose sequence.

Parameters:

index – first element position inside the internal PoseSequence
number – number of elements that should be deleted

void deleteTimeDiff(int index)

Delete pose at pos. index in the timediff sequence.

Parameters:: index – element position inside the internal TimeDiffSequence

void deleteTimeDiffs(int index, int number)

Delete multiple (number) time differences starting at pos. index in the timediff sequence.

Parameters:

index – first element position inside the internal TimeDiffSequence
number – number of elements that should be deleted

Init the trajectory

bool initTrajectoryToGoal(const PoseSE2 &start, const PoseSE2 &goal, double diststep = 0, double max_vel_x = 0.5, int min_samples = 3, bool guess_backwards_motion = false)

Initialize a trajectory between a given start and goal pose.

The implemented algorithm subsamples the straight line between start and goal using a given discretiziation width.

The discretization width can be defined in the euclidean space using the

diststep parameter. Each time difference between two consecutive poses is initialized to timestep

.

If the

diststep is chosen to be zero, the resulting trajectory contains the start and goal pose only.

Parameters:

start – PoseSE2 defining the start of the trajectory
goal – PoseSE2 defining the goal of the trajectory (final pose)
diststep – euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
max_vel_x – maximum translational velocity used for determining time differences
min_samples – Minimum number of samples that should be initialized at least
guess_backwards_motion – Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot

Returns:

true if everything was fine, false otherwise

template<typename BidirIter, typename Fun> bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta, boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false)

Initialize a trajectory from a generic 2D reference path.

The temporal information is determined using a given maximum velocity (2D vector containing the translational and angular velocity).

A constant velocity profile is implemented.

A possible maximum acceleration is considered if max_acceleration param is provided.

Since the orientation is not included in the reference path, it can be provided seperately (e.g. from the robot pose and robot goal).

Otherwise the goal heading will be used as start and goal orientation.

The orientation along the trajectory will be determined using the connection vector between two consecutive positions of the reference path.

The reference path is provided using a start and end iterator to a container class. You must provide a unary function that accepts the dereferenced iterator and returns a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.

Remark

Use boost::none to skip optional arguments.

Parameters:

path_start – start iterator of a generic 2d path
path_end – end iterator of a generic 2d path
fun_position – unary function that returns the Eigen::Vector2d object
max_vel_x – maximum translational velocity used for determining time differences
max_vel_theta – maximum angular velocity used for determining time differences
max_acc_x – specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
max_acc_theta – specify to satisfy a maxmimum angular acceleration and decceleration (optional)
start_orientation – Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
goal_orientation – Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
min_samples – Minimum number of samples that should be initialized at least
guess_backwards_motion – Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot

Template Parameters:

BidirIter – Bidirectional iterator type
Fun – unyary function that transforms the dereferenced iterator into an Eigen::Vector2d

Returns:

true if everything was fine, false otherwise

bool initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped> &plan, double max_vel_x, double max_vel_theta, bool estimate_orient = false, int min_samples = 3, bool guess_backwards_motion = false, double skip_dist = 0.0)

Initialize a trajectory from a reference pose sequence (positions and orientations).

This method initializes the timed elastic band using a pose container (e.g. as local plan from the ros navigation stack).

The initial time difference between two consecutive poses can be uniformly set via the argument

dt.

Parameters:

plan – vector of geometry_msgs::PoseStamped
max_vel_x – maximum translational velocity used for determining time differences
max_vel_theta – maximum rotational velocity used for determining time differences
estimate_orient – if true, calculate orientation using the straight line distance vector between consecutive poses (only copy start and goal orientation; recommended if no orientation data is available).
min_samples – Minimum number of samples that should be initialized at least
guess_backwards_motion – Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if estimate_orient is enabled.

Returns:

true if everything was fine, false otherwise

inline ROS_DEPRECATED bool initTEBtoGoal (const PoseSE2 &start, const PoseSE2 &goal, double diststep=0, double timestep=1, int min_samples=3, bool guess_backwards_motion=false)

template<typename BidirIter, typename Fun> inline ROS_DEPRECATED bool initTEBtoGoal (BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, boost::optional< double > max_acc_x, boost::optional< double > max_acc_theta, boost::optional< double > start_orientation, boost::optional< double > goal_orientation, int min_samples=3, bool guess_backwards_motion=false)

inline ROS_DEPRECATED bool initTEBtoGoal (const std::vector< geometry_msgs::PoseStamped > &plan, double dt, bool estimate_orient=false, int min_samples=3, bool guess_backwards_motion=false)

Update and modify the trajectory

void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3)

Hot-Start from an existing trajectory with updated start and goal poses.

This method updates a previously optimized trajectory with a new start and/or a new goal pose.

The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start.

Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed. The optimizer has to smooth the trajectory in TebOptimalPlanner.

Parameters:

new_start – New start pose (optional)
new_goal – New goal pose (optional)
min_samples – Specify the minimum number of samples that should at least remain in the trajectory

void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples = 1000, bool fast_mode = false)

Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.

Resizing the trajectory is helpful e.g. for the following scenarios:

- Obstacles requires the teb to be extended in order to
  satisfy the given discritization width (plan resolution)
 and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$
  After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.

If the distance to the goal state is getting smaller, dt is decreasing as well. This leads to a heavily fine-grained discretization in combination with many discrete poses. Thus, the computation time will be/remain high and in addition numerical instabilities can appear (e.g. due to the division by a small \( \Delta T_i \)).

The implemented strategy checks all timediffs \( \Delta T_i \) and

- inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$

removes a sample if \( \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \)

Each call only one new sample (pose-dt-pair) is inserted or removed.

Parameters:

dt_ref – reference temporal resolution
dt_hysteresis – hysteresis to avoid oscillations
min_samples – minimum number of samples that should be remain in the trajectory after resizing
max_samples – maximum number of samples that should not be exceeded during resizing
fast_mode – if true, the trajectory is iterated once to insert or erase points; if false the trajectory is repeatedly iterated until no poses are added or removed anymore

void setPoseVertexFixed(int index, bool status)

Set a pose vertex at pos index of the pose sequence to be fixed or unfixed during optimization.

Parameters:

index – index to the pose vertex
status – if true, the vertex will be fixed, otherwise unfixed

void setTimeDiffVertexFixed(int index, bool status)

Set a timediff vertex at pos index of the timediff sequence to be fixed or unfixed during optimization.

Parameters:

index – index to the timediff vertex
status – if true, the vertex will be fixed, otherwise unfixed

void clearTimedElasticBand(): clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and isInit() will return false

Public Functions

TimedElasticBand(): Construct the class.

virtual ~TimedElasticBand(): Destruct the class.