search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.06k stars 550 forks source link

Incorrect curvature calculation in EdgeKinematicsCarlike? #347

Closed VRichardJP closed 2 years ago

VRichardJP commented 2 years ago

I am curious about the formula used to calculate curvature in EdgeKinematicsCarlike https://github.com/rst-tu-dortmund/teb_local_planner/blob/3bcbd97360b6db33d81d1d354b5b4cdbc3d7018b/include/teb_local_planner/g2o_types/edge_kinematics.h#L212-L215

It seems to be based Menger curvature but the calculation in TEB differs by having an extra /2 factor: TEB uses fabs(deltaS.norm()/(2*sin(angle_diff/2) while it should rather look like fabs(deltaS.norm()/(2*sin(angle_diff) according to wikipedia's formula

Where does this extra /2 come from?

Similarly, it seems that the formula for cfg_->trajectory.exact_arc_length=false is simply a first order approximation of the previous formula for when angle_diff≈0, but this condition is not checked. If my understanding is correct, most of the time we can expect angle_diff to be close to 0 (forward/backward driving). So the first order approximation would be deltaS.norm() / 2*fabs(angle_diff).

Or is my understanding incorrect?

VRichardJP commented 2 years ago

My bad, the calculation is correct.

Here is an example construction, where A and C are the origin of 2 vectors and B is placed on the circle so that ABC is an isosceles triangle. The ABC angle is then pi-angle_diff/2 and we get the correct result with the Menger formula. curvature