updates to motion planning config

hello-robot · Oct 10, 2024 · 4065465 · 4065465
1 parent fbcf472
commit 4065465
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 2 deletions.
diff --git a/src/stretch/agent/robot_agent.py b/src/stretch/agent/robot_agent.py
@@ -151,7 +151,13 @@ def __init__(
         if parameters["motion_planner"]["shortcut_plans"]:
             self.planner = Shortcut(self.planner, parameters["motion_planner"]["shortcut_iter"])
         if parameters["motion_planner"]["simplify_plans"]:
-            self.planner = SimplifyXYT(self.planner, min_step=0.05, max_step=1.0, num_steps=8)
+            self.planner = SimplifyXYT(
+                self.planner,
+                min_step=parameters["motion_planner"]["simplify"]["min_step"],
+                max_step=parameters["motion_planner"]["simplify"]["max_step"],
+                num_steps=parameters["motion_planner"]["simplify"]["num_steps"],
+                min_angle=parameters["motion_planner"]["simplify"]["min_angle"],
+            )
 
         if self._realtime_updates:
             # Locks

diff --git a/src/stretch/config/default_planner.yaml b/src/stretch/config/default_planner.yaml
@@ -116,6 +116,11 @@ motion_planner:
   rotation_step_size: 0.1
   simplify_plans: True
   shortcut_plans: True
+  simplify:
+    max_step: 0.5
+    min_step: 0.05
+    num_steps: 8
+    min_angle: 0.1
   shortcut_iter: 100
   # Parameters for frontier exploration using the motion planner.
   frontier:

diff --git a/src/stretch/motion/algo/simplify.py b/src/stretch/motion/algo/simplify.py
@@ -36,7 +36,7 @@ def __init__(
         self,
         planner: Planner,
         min_step: float = 0.1,
-        max_step: float = 1.0,
+        max_step: float = 0.5,
         num_steps: int = 6,
         min_angle: float = np.deg2rad(5),
     ):