Trajectory Class Reference
[Trajectory Generator]

Represents the whole trajectory. More...

#include <trgen.h>

Inheritance diagram for Trajectory:

Inheritance graph
[legend]
Collaboration diagram for Trajectory:

Collaboration graph
[legend]

List of all members.

Public Member Functions

 Trajectory (TrajectoryConstraints tc, bool _backward=false)
 Trajectory constructor.
 ~Trajectory ()
void addPoint (double x, double y)
 Adds a new point to the trajectory.
void addTurn (double angle)
double calcLength ()
 Returns the length of the whole trajectory.
bool prepare (Pos _initPos)
 Prepares the trajectory.
bool getRefPos (double time, Pos &rp)
 Returns reference position of the trajectory at a given time.

Public Attributes

double finalHeading
 Desired heading of the robot at the end of the trajectory.
bool backward

Private Member Functions

void deleteSegments ()
bool splitSegment (iterator &seg, double distance)
 Split a sergment and add the newly created segment to the trajectory.
bool points2segments ()
 Connects points by line segments.
void corners2arcs ()
 Converts corners in trajectory to arcs.
void addTurns ()
void calcSpeeds ()
 Assigns speeds and accelerations to segments; tries to maximize speed where possible.
void getSegRefPos (TrajectorySegment *seg, double time, Pos &rp)
 Returns reference position of the given segment in the trajectory and takes backward into the account.

Private Attributes

Pos initPos
iterator currentSeg
TrajectoryPoints wayPoints
 Points, this trajectory is composed from.
Point * initialPoint
Point * finalPoint
TrajectoryConstraints constr
bool prepared

Detailed Description

Represents the whole trajectory.

When a new trajectory is created (by Trajectory() constructor), addPoint() should be used to add points, the trajectory should go through. After all points are added, a call to prepare() uses these points to construct optimized trajectory and find the speeds along the trajectory. After the trajectory is prepared, you can call getRefPos(), to get the reference position at a specific time.

Definition at line 221 of file trgen.h.

Constructor & Destructor Documentation

Trajectory::Trajectory ( TrajectoryConstraints  tc,
bool  _backward = false 
) [inline]

Trajectory constructor.

Parameters:
tc Trajectory constraints
_backward Whether the trajectory should be followed in backward direction.

Definition at line 256 of file trgen.h.

Member Function Documentation

bool Trajectory::splitSegment ( iterator &  seg,
double  distance 
) [private]

Split a sergment and add the newly created segment to the trajectory.

Parameters:
seg Segment to split.
distance Where to split.
Returns:

Definition at line 345 of file trgen.cc.

References dbgPrintf, TrajectorySegment::splitAt(), and wayPoints.

Referenced by calcSpeeds().

Here is the call graph for this function:

Here is the caller graph for this function:

void Trajectory::corners2arcs (  )  [private]

Converts corners in trajectory to arcs.

Todo:
Splines will be better.

Definition at line 417 of file trgen.cc.

References constr, TrajectoryConstraints::maxe, and wayPoints.

Referenced by prepare().

Here is the caller graph for this function:

void Trajectory::calcSpeeds (  )  [private]

Assigns speeds and accelerations to segments; tries to maximize speed where possible.

If necessary it also splits long segments to multiple parts with maximum acceleration/deceleration and maximal speed. The optimization is done in three passes. In the first pass, maximal speed of segments are set up (see TrajectoryConstraints::maxv, TrajectoryConstraints::maxomega, TrajectoryConstraints::maxcenacc). Then follows a forward pass, where acceleration is added to join segments with increasing maximal speeds. Finally, backward pass does almost the same by adding deceleration segments.

Finding the split point of the segment where deceleration immediately follows acceleration is based on the following calculations:

For uniformly accelerated motion, we can calculate time as function of speed:

\[ t(v)=\frac{v-v_0}{a}\]

Using this, we can express distance $ s $ as a function of speed:

\[ s(v) = \frac{1}{2} at(v)^2 + v_0 t(v) = \frac{v^2 - v_0^2}{2a}\]

Then, distance traveled during acceleration ($ s_1 $) and deceleration ($ s_2 $) must be equal to the length of the segment $ l = s_1 + s_2 $. Considering a segment with initial speed $ v_1 $, final speed $ v_2 $, and maximal speed (in the split point) $ v_{max} $, we can derive:

\[ l = s_1(v_{max}) + s_2(v_{max}) = \frac{2v_{max}^2 - v_1^2 - v_2^2}{2a} \]

\[ v_{max}^2 = al + \frac{v_1^2 + v_2^2}{2} \]

Then, the length of the segment before split is:

\[ s_1 = l - s_2 = l - \frac{v_{max}^2-v_2^2}{2a} = \frac{1}{2}l + \frac{v_2^2 - v_1^2}{4a} \]

Definition at line 563 of file trgen.cc.

References constr, dbgPrintf, initPos, splitSegment(), and Pos::v.

Referenced by prepare().

Here is the call graph for this function:

Here is the caller graph for this function:

void Trajectory::getSegRefPos ( TrajectorySegment *  seg,
double  time,
Pos &  rp 
) [private]

Returns reference position of the given segment in the trajectory and takes backward into the account.

Parameters:
[in] seg Segment to get the position from.
[in] time Time from the beginning of the trajectory.
[out] rp Returned reference position.

Definition at line 858 of file trgen.cc.

References backward, TrajectorySegment::getRefPos(), Pos::phi, and Pos::v.

Referenced by getRefPos().

Here is the call graph for this function:

Here is the caller graph for this function:

void Trajectory::addPoint ( double  x,
double  y 
) [inline]

Adds a new point to the trajectory.

Parameters:
x 
y 

Definition at line 270 of file trgen.h.

References finalPoint, and wayPoints.

Referenced by new_goal(), and robot_trajectory_add_point().

Here is the caller graph for this function:

double Trajectory::calcLength (  ) 

Returns the length of the whole trajectory.

Definition at line 694 of file trgen.cc.

bool Trajectory::prepare ( Pos  _initPos  ) 

Prepares the trajectory.

It converts points added by addPoint() to line segments (Line). Then it replaces by sharp corners by arcs (see corners2arcs()), then it adds turns at the begin and end and finally it calculates speeds in the different parts of the trajectory by adding acceleration and decceleration segments (see calcSpeeds()).

Parameters:
_initPos The point where the trajectory should start. Typically the current robot position.
Todo:
Solve properly the case, when initPos has non-zero speed.
Returns:
True in case of success, false when the trajectory has only one point and initPos is already equal to that point.

Definition at line 771 of file trgen.cc.

References addTurns(), calcSpeeds(), constr, corners2arcs(), currentSeg, dbgPrintf, initPos, TrajectoryConstraints::maxacc, TrajectoryConstraints::maxangacc, TrajectoryConstraints::maxcenacc, TrajectoryConstraints::maxe, TrajectoryConstraints::maxomega, TrajectoryConstraints::maxv, points2segments(), prepared, and wayPoints.

Referenced by new_goal(), and new_trajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

bool Trajectory::getRefPos ( double  time,
Pos &  rp 
)

Returns reference position of the trajectory at a given time.

Parameters:
[in] time Time from the beginning of the trajectory.
[out] rp Returned reference position.
Returns:
True if the time is after the trajectory ends or error. False otherwise.

Definition at line 817 of file trgen.cc.

References currentSeg, getSegRefPos(), TrajectorySegment::getT1(), TrajectorySegment::getT2(), and prepared.

Referenced by trajectory_follower().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

TrajectoryPoints Trajectory::wayPoints [private]

Points, this trajectory is composed from.

Definition at line 224 of file trgen.h.

Referenced by addPoint(), corners2arcs(), points2segments(), prepare(), and splitSegment().

double Trajectory::finalHeading

Desired heading of the robot at the end of the trajectory.

If equal to NAN, ignored.

Definition at line 247 of file trgen.h.

Referenced by addTurns(), new_goal(), and robot_trajectory_add_final_point().

The documentation for this class was generated from the following files:
Generated on Thu Sep 13 11:28:29 2007 for DCE-Eurobot by  doxygen 1.5.3