Delta angle oscillating between -PI and +PI radians when projection is close to parallel with LOS

[rwbot]

[rwbot]

Example case:

DIST TO GOAL = 0.0803 < 0.03
> GETTING DELTA ANGLE -0.54776
PHI = YAW (0.794795) + delta (-0.54776) = 0.247035
GETTING DELTA CONTROL = atan(- 20 * 0.247035) = atan(- 4.94071) = -1.37109
GETTING DELTA CONTROL -1.37109
Calculating OMEGA with Constant Linear Velocity ->0.1
GETTING Z = -0.54776 - -1.37109 = 0.823334
GETTING LEFT = 1 * 0.823334 = 0.823334
GETTING OMEGA = 0.823334 + -0.930665 = -0.107331
GETTING OMEGA = 1.33663 * 0.1 = 0.133663
CALCULATING OMEGA DIRECTLY 0.133663
BOUNDING OMEGA TO 0.133663
ADDING POSE TO PLAN[ 0.50, 0.01, 0.00 ] [ 0.000, 0.000, -0.146, 0.989 ] YAW: -0.292
Started Stepping Pose with Twist [ X: 0.100, Z: 0.134 ]
DELTA X POSITION 0.01 DELTA Y POSITION 0.00
ORIG TF [ 0.50, 0.01, 0.00 ] [ 0.000, 0.000, -0.146, 0.989 ] YAW: -0.29
DELTA TF [ 0.01, 0.00, 0.00 ] [ 0.000, 0.000, 0.007, 1.000 ] YAW: 0.01
STEPPED [ 0.51, 0.01, 0.00 ] [ 0.000, 0.000, -0.139, 0.990 ] YAW: -0.279
GETTING DISTANCE BETWEEN UPDATED TARGET & STEPPED POSE
GETTING TF FROM PROJECTION TO TARGET [ -0.00, -0.01, -0.08 ] [ 0.0, 0.003, 0.381, 0.925 ]
GOAL DISTANCE OLD: 0.0803 NEW: 0.0802
END PATH STEP #50

---

---

DIST TO GOAL = 0.0802425 < 0.03
> GETTING DELTA ANGLE 1.87089
PHI = YAW (0.781429) + delta (1.87089) = 2.65232
GETTING DELTA CONTROL = atan(- 20 * 2.65232) = atan(- 53.0465) = -1.55195
GETTING DELTA CONTROL -1.55195
Calculating OMEGA with Constant Linear Velocity ->0.1
GETTING Z = 1.87089 - -1.55195 = 3.42284
GETTING LEFT = 1 * 3.42284 = 3.42284
GETTING OMEGA = 3.42284 + 0.962095 = 4.38494
GETTING OMEGA = -54.6461 * 0.1 = -5.46461
CALCULATING OMEGA DIRECTLY -5.46461
BOUNDING OMEGA TO -1
ADDING POSE TO PLAN[ 0.51, 0.01, 0.00 ] [ 0.0, 0.0, -0.139, 0.990 ] YAW: -0.279
Started Stepping Pose with Twist [ X: 0.100, Z: -1.000 ]
DELTA X POSITION 0.01 DELTA Y POSITION -0.00
ORIG TF [ 0.51, 0.01, 0.00 ] [ 0.000, 0.000, -0.139, 0.990 ] YAW: -0.28
DELTA TF [ 0.01, -0.00, 0.00 ] [ 0.000, 0.000, -0.050, 0.999 ] YAW: -0.10
STEPPED [ 0.52, -0.04, 0.00 ] [ 0.000, 0.000, -0.188, 0.982 ] YAW: -0.379
GETTING DISTANCE BETWEEN UPDATED TARGET & STEPPED POSE
GETTING TF FROM PROJECTION TO TARGET [ -0.03, 0.04, -0.08 ] [ 0.0, 0.003, 0.427, 0.904 ]
GOAL DISTANCE OLD: 0.0802 NEW: 0.0934
END PATH STEP #51

---

---

DIST TO GOAL = 0.0933906 < 0.03
> GETTING DELTA ANGLE -2.2038
PHI = YAW (0.881429) + delta (-2.2038) = -1.32238
GETTING DELTA CONTROL = atan(- 20 * -1.32238) = atan(- -26.4475) = 1.533
GETTING DELTA CONTROL 1.533
Calculating OMEGA with Constant Linear Velocity ->0.1
GETTING Z = -2.2038 - 1.533 = -3.73681
GETTING LEFT = 1 * -3.73681 = -3.73681
GETTING OMEGA = -3.73681 + -0.829272 = -4.56608
GETTING OMEGA = 48.8923 * 0.1 = 4.88923
CALCULATING OMEGA DIRECTLY 4.88923
BOUNDING OMEGA TO 1
ADDING POSE TO PLAN[ 0.52, -0.04, 0.00 ] [ 0.0, 0.0, -0.188, 0.982 ] YAW: -0.379
Started Stepping Pose with Twist [ X: 0.100, Z: 1.000 ]
DELTA X POSITION 0.01 DELTA Y POSITION 0.00
ORIG TF [ 0.52, -0.04, 0.00 ] [ 0.000, 0.000, -0.188, 0.982 ] YAW: -0.38
DELTA TF [ 0.01, 0.00, 0.00 ] [ 0.000, 0.000, 0.050, 0.999 ] YAW: 0.10
STEPPED [ 0.53, 0.01, 0.00 ] [ 0.000, 0.000, -0.139, 0.990 ] YAW: -0.279
GETTING DISTANCE BETWEEN UPDATED TARGET & STEPPED POSE
GETTING TF FROM PROJECTION TO TARGET [ -0.02, -0.01, -0.08 ] [ 0.0, 0.003, 0.381, 0.925 ]
GOAL DISTANCE OLD: 0.0934 NEW: 0.0835
END PATH STEP #52

---

---

DIST TO GOAL = 0.0835465 < 0.03
> GETTING DELTA ANGLE 2.6079
PHI = YAW (0.781429) + delta (2.6079) = 3.38933
GETTING DELTA CONTROL = atan(- 20 * 3.38933) = atan(- 67.7866) = -1.55605
GETTING DELTA CONTROL -1.55605
Calculating OMEGA with Constant Linear Velocity ->0.1
GETTING Z = 2.6079 - -1.55605 = 4.16395
GETTING LEFT = 1 * 4.16395 = 4.16395
GETTING OMEGA = 4.16395 + 0.510928 = 4.67488
GETTING OMEGA = -55.9554 * 0.1 = -5.59554
CALCULATING OMEGA DIRECTLY -5.59554
BOUNDING OMEGA TO -1
ADDING POSE TO PLAN[ 0.53, 0.01, 0.00 ] [ 0.000, 0.000, -0.139, 0.990 ] YAW: -0.279
Started Stepping Pose with Twist [ X: 0.100, Z: -1.000 ]
DELTA X POSITION 0.01 DELTA Y POSITION -0.00
ORIG TF [ 0.53, 0.01, 0.00 ] [ 0.000, 0.000, -0.139, 0.990 ] YAW: -0.28
DELTA TF [ 0.01, -0.00, 0.00 ] [ 0.000, 0.000, -0.050, 0.999 ] YAW: -0.10
STEPPED [ 0.54, -0.04, 0.00 ] [ 0.000, 0.000, -0.188, 0.982 ] YAW: -0.379
GETTING DISTANCE BETWEEN UPDATED TARGET & STEPPED POSE
GETTING TF FROM PROJECTION TO TARGET [ -0.05, 0.03, -0.08 ] [ 0.000, 0.003, 0.427, 0.904 ]
GOAL DISTANCE OLD: 0.0835 NEW: 0.0985
END PATH STEP #53

---

---

FINISHED 54 STEPS
COMPLETED TRAJECTORY CALCULATION