Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. More...
#include <edge_acceleration.h>
Public Member Functions | |
void | computeError () |
Actual cost function. | |
EdgeAccelerationGoal () | |
Construct edge. | |
void | setGoalVelocity (const geometry_msgs::Twist &vel_goal) |
Set the goal / final velocity that is taken into account for calculating the acceleration. |
Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
The edge depends on three vertices , an initial velocity defined by setGoalVelocity() and minimizes:
.
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.
Definition at line 382 of file edge_acceleration.h.
Construct edge.
Definition at line 389 of file edge_acceleration.h.
void teb_local_planner::EdgeAccelerationGoal::computeError | ( | ) | [inline] |
Actual cost function.
Definition at line 399 of file edge_acceleration.h.
void teb_local_planner::EdgeAccelerationGoal::setGoalVelocity | ( | const geometry_msgs::Twist & | vel_goal | ) | [inline] |
Set the goal / final velocity that is taken into account for calculating the acceleration.
vel_goal | twist message containing the translational and rotational velocity |
Definition at line 443 of file edge_acceleration.h.