trajectory_tracker: vibration around a goal

ykoga-kyutech commented 5 years ago

When a robot is about to reach a goal of a path, the robot vibrates around the goal. Might be trajectory tracker problem.

Launched with the following parameters.

spur:
  curv_forward: 0.2
  look_forward: 0.0
  k_dist: 2.5
  dist_lim: 0.5
  k_ang: 5.0
  k_avel: 4.0
  path_step: 1
  limit_vel_by_avel: true
  hz: 30.0
  distance_angle_factor: 0.2
  rotate_ang: 0.5
  no_position_control_dist: 0.0
  goal_tolerance_dist: 0.01
  stop_tolerance_dist: 0.01
  goal_tolerance_ang: 0.01
  stop_tolerance_ang: 0.01
  acc_toc_factor: 0.9
  angacc_toc_factor: 0.9

When the robot vibrates, distance_reamins value on /spur/status topic changes from -0.06 ~ 0.06. The path is generated by my own program. Resolution of the path is 0.01 and size of the remaining pose of the path while vibrating is 3 as like shown below:

``` $ rostopic echo /path header: seq: 5185 stamp: secs: 1547722181 nsecs: 158192990 frame_id: "odom" poses: - header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 1.53188276524 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 - header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 1.56188276524 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 - header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 1.565 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 --- ```

at-wat commented 5 years ago

The bug can be reproduced by the test added by #272. It looks that 3 poses path can be treated correctly. For 2 poses path, remained distance has opposite sign and vibrates due to it.

ykoga-kyutech commented 5 years ago

The vibration does not occur after applying the change #272

For 2 poses path, remained distance has opposite sign and vibrates due to it.

I see, I've found that both 2 and 3 pose are published, while vibrating. Thanks for your rapid support.

at-wat / neonavigation

trajectory_tracker: vibration around a goal #271