Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. More...

#include <edge_acceleration.h>

Inheritance diagram for teb_local_planner::EdgeAccelerationStart:

[legend]

List of all members.

Public Member Functions
void	computeError ()
	Actual cost function.
	EdgeAccelerationStart ()
	Construct edge.
void	setInitialVelocity (const geometry_msgs::Twist &vel_start)
	Set the initial velocity that is taken into account for calculating the acceleration.

Detailed Description

Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.

The edge depends on three vertices $\mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i$ , an initial velocity defined by setInitialVelocity() and minimizes:
$\min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight$ .
a is calculated using the difference quotient (twice) and the position parts of the poses.
omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi].
weight can be set using setInformation().
penaltyInterval denotes the penalty function, see penaltyBoundToInterval().
The dimension of the error / cost vector is 2: the first component represents the translational acceleration and the second one the rotational acceleration.