Create Planner Benchmark, set planner to RRT* (#204)

* Switch default to RRT*

* Switch default to  Anytime Path Shortening w/ RRTConnect

* [WIP] add planner benchmark

* Finalized planner benchmark

* Add timeout, improve logging

* Run pre-commit

* Revert to RRT*, percolate updated allowed planning time into AcquireFood

Author: amalnanavati

Diff for: ada_feeding/CMakeLists.txt

@@ -36,6 +36,7 @@ install(PROGRAMS
   scripts/ada_watchdog.py
   scripts/dummy_ft_sensor.py
   scripts/joint_state_latency.py
+  scripts/planner_benchmark.py
   scripts/robot_state_service.py
   scripts/save_image.py
   DESTINATION lib/${PROJECT_NAME}

Diff for: ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py

@@ -93,7 +93,7 @@ def blackboard_inputs(
         ] = None,
         ignore_violated_path_constraints: Union[BlackboardKey, bool] = False,
         pipeline_id: Union[BlackboardKey, str] = "ompl",
-        planner_id: Union[BlackboardKey, str] = "RRTConnectkConfigDefault",
+        planner_id: Union[BlackboardKey, str] = "RRTstarkConfigDefault",
         allowed_planning_time: Union[BlackboardKey, float] = 0.5,
         max_velocity_scale: Union[BlackboardKey, float] = 0.1,
         max_acceleration_scale: Union[BlackboardKey, float] = 0.1,
@@ -538,6 +538,7 @@ def get_path_len(
         for point in path.points:
             curr_pos = np.array(point.positions)
             seg_len = np.abs(curr_pos - prev_pos)
+            seg_len = np.minimum(seg_len, 2 * np.pi - seg_len)
             if j6_i is not None:
                 j6_len = seg_len[j6_i]
                 seg_len[j6_i] = 0.0

Diff for: ada_feeding/ada_feeding/trees/acquire_food_tree.py

@@ -92,7 +92,7 @@ def __init__(
         max_velocity_scaling_to_resting_configuration: Optional[float] = 0.8,
         max_acceleration_scaling_to_resting_configuration: Optional[float] = 0.8,
         pickle_goal_path: Optional[str] = None,
-        allowed_planning_time_for_move_above: float = 2.0,
+        allowed_planning_time_for_move_above: float = 0.5,
         allowed_planning_time_for_move_into: float = 0.5,
         allowed_planning_time_to_resting_configuration: float = 0.5,
         allowed_planning_time_for_recovery: float = 0.5,

Diff for: ada_feeding/ada_feeding/trees/move_from_mouth_tree.py

@@ -69,7 +69,7 @@ def __init__(
         orientation_constraint_to_end_configuration_tolerances: Optional[
             List[float]
         ] = None,
-        planner_id: str = "RRTConnectkConfigDefault",
+        planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time_to_staging_configuration: float = 0.5,
         allowed_planning_time_to_end_configuration: float = 0.5,
         max_linear_speed_to_staging_configuration: float = 0.05,

Diff for: ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py

@@ -48,7 +48,7 @@ def __init__(
         tolerance_joint: float = 0.001,
         weight_joint: float = 1.0,
         pipeline_id: str = "ompl",
-        planner_id: str = "RRTConnectkConfigDefault",
+        planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time: float = 0.5,
         max_velocity_scaling_factor: float = 0.1,
         max_acceleration_scaling_factor: float = 0.1,

Diff for: ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py

@@ -61,7 +61,7 @@ def __init__(
         goal_configuration_tolerance: float = 0.001,
         orientation_constraint_quaternion: Optional[List[float]] = None,
         orientation_constraint_tolerances: Optional[List[float]] = None,
-        planner_id: str = "RRTConnectkConfigDefault",
+        planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time: float = 0.5,
         max_velocity_scaling_factor: float = 0.1,
         force_threshold: float = 4.0,

Diff for: ada_feeding/ada_feeding/trees/move_to_pose_tree.py

@@ -58,7 +58,7 @@ def __init__(
         cartesian_max_step: float = 0.0025,
         cartesian_fraction_threshold: float = 0.0,
         pipeline_id: str = "ompl",
-        planner_id: str = "RRTConnectkConfigDefault",
+        planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time: float = 0.5,
         max_velocity_scaling_factor: float = 0.1,
         max_acceleration_scaling_factor: float = 0.1,