nasa / astrobee

NASA Astrobee Robot Software
https://www.nasa.gov/astrobee
Apache License 2.0
1.06k stars 312 forks source link

QP planner fails to plan face forward mode trajectories #24

Closed Kevinkald closed 4 years ago

Kevinkald commented 4 years ago

After using the QP planner for some time I've noticed that there is some weird behaviors when using this planner. These weird behaviors are related to attitude, not positions.

  1. When Astrobee is following a generated trajectory by the QP planner, the error of the attitude feedback from the controller exceeds the threshold in iss.config, parameter tolerance_att. This causes the choreographer to stop the execution of the trajectory.

  2. When Astrobee has approached the goal position, the robot starts to do some weird attitude maneuvers. And when the trajectory is finished, Astrobee is rarely on the desired end orientation.

I have looked into the problems, and have come to the following results:

enable_faceforward = false These two plots shows that the generated trajectory in blue converges with the desired positions and quaternions in green. face_forward_off_position face_forward_off_quaternion

enable_faceforward = true These two plots shows that the generated positions in blue converges with the desired positions in green. The quaternions however, does not converge to the desired quaternions in green. Also notice the non-smooth trajectories for q_z and q_w. This also helps to explain the problem listed in 1. above. face_forward_enabled_position face_forward_enabled_quaternion

Conclusion The QP planner fails to generate smooth trajectories satisfying the end attitude constraints when face forward mode is enabled. This seems to be a problem with the planner, and not something else. Since the return to dock behavior and re planning with obstacle avoidance uses the QP planner, this issue should be fixed in order to use these behaviors responsibly.

Kevinkald commented 4 years ago

Issue solved in commit 51edd44c7c18c941b143e9908cef28709258ec5f

There is still some issues with attitude tolerance violation in the controller, which cancels the move action. Will investigate this new issue further.