search

rwbot / docking

Development of ROS package that provides generalized docking solution for LiDAR equipped mobile robots
31 stars 18 forks source link

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

Open rwbot opened 4 years ago

rwbot commented 4 years ago

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