Improving planners and experiments

Author: Federico-PizarroBejarano

mm_plan/src/mm_plan/Planners.py

@@ -5,6 +5,7 @@
 
 import numpy as np
 
+from mm_utils import parsing
 from mm_utils.enums import RefType
 from mm_utils.math import interpolate, wrap_pi_array, wrap_pi_scalar
 
@@ -142,7 +143,7 @@ def __init__(self, config):
 
         # Parse base pose (optional)
         if "base_pose" in config:
-            self.base_target = np.array(config["base_pose"])
+            self.base_target = parsing.parse_array(config["base_pose"])
             if len(self.base_target) != 3:
                 raise ValueError(
                     f"base_pose must be SE2 [x, y, yaw], got {len(self.base_target)} dimensions"
@@ -154,7 +155,7 @@ def __init__(self, config):
 
         # Parse EE pose (optional)
         if "ee_pose" in config:
-            self.ee_target = np.array(config["ee_pose"])
+            self.ee_target = parsing.parse_array(config["ee_pose"])
             if len(self.ee_target) != 6:
                 raise ValueError(
                     f"ee_pose must be SE3 [x, y, z, roll, pitch, yaw], got {len(self.ee_target)} dimensions"
@@ -170,10 +171,23 @@ def __init__(self, config):
             )
 
         # Common parameters
-        self.tracking_err_tol = config.get("tracking_err_tol", 0.02)
+        self.tracking_pos_err_tol = config.get("tracking_pos_err_tol", 0.02)
         self.tracking_ori_err_tol = config.get("tracking_ori_err_tol", 0.1)
         self.hold_period = config.get("hold_period", 0.0)
 
+        # Parse masks to specify which dimensions matter for completion
+        # base_mask: [x, y, yaw] - True means that dimension is checked
+        if "base_mask" in config:
+            self.base_mask = np.array(config["base_mask"], dtype=bool)
+        else:
+            self.base_mask = np.ones(3, dtype=bool)
+
+        # ee_mask: [x, y, z, roll, pitch, yaw] - True means that dimension is checked
+        if "ee_mask" in config:
+            self.ee_mask = np.array(config["ee_mask"], dtype=bool)
+        else:
+            self.ee_mask = np.ones(6, dtype=bool)
+
         # State tracking
         self.finished = False
         self.base_reached = False
@@ -224,10 +238,19 @@ def checkFinished(self, t, states):
         # Check base if applicable
         if self.has_base_ref:
             base_pose = states["base"]["pose"]
-            pos_err = np.linalg.norm(base_pose[:2] - self.base_target[:2])
-            yaw_err = abs(wrap_pi_scalar(base_pose[2] - self.base_target[2]))
-            pos_within_tol = pos_err < self.tracking_err_tol
-            ori_within_tol = yaw_err < self.tracking_ori_err_tol
+
+            # Check position (x, y) only if mask indicates it matters
+            pos_mask = self.base_mask[:2]
+            pos_err = np.linalg.norm((base_pose[:2] - self.base_target[:2])[pos_mask])
+            pos_within_tol = pos_err < self.tracking_pos_err_tol
+
+            # Check orientation (yaw) only if mask indicates it matters
+            if self.base_mask[2]:
+                yaw_err = abs(wrap_pi_scalar(base_pose[2] - self.base_target[2]))
+                ori_within_tol = yaw_err < self.tracking_ori_err_tol
+            else:
+                ori_within_tol = True
+                yaw_err = 0.0
 
             if pos_within_tol and ori_within_tol:
                 if not self.base_reached:
@@ -246,10 +269,16 @@ def checkFinished(self, t, states):
         # Check EE if applicable
         if self.has_ee_ref:
             ee_pose = states["EE"]["pose"]
-            pos_err = np.linalg.norm(ee_pose[:3] - self.ee_target[:3])
+
+            # Check position (x, y, z) only if mask indicates it matters
+            pos_mask = self.ee_mask[:3]
+            pos_err = np.linalg.norm((ee_pose[:3] - self.ee_target[:3])[pos_mask])
+            pos_within_tol = pos_err < self.tracking_pos_err_tol
+
+            # Check orientation (roll, pitch, yaw) only if mask indicates it matters
+            ori_mask = self.ee_mask[3:]
             ori_diff = wrap_pi_array(ee_pose[3:] - self.ee_target[3:])
-            ori_err = np.linalg.norm(ori_diff)
-            pos_within_tol = pos_err < self.tracking_err_tol
+            ori_err = np.linalg.norm(ori_diff[ori_mask])
             ori_within_tol = ori_err < self.tracking_ori_err_tol
 
             if pos_within_tol and ori_within_tol:
@@ -305,7 +334,10 @@ def __init__(self, config):
 
         # Parse base path (optional)
         if "base_path" in config:
-            base_path = np.array(config["base_path"])
+            # Parse each row to handle pi notation
+            base_path = np.array(
+                [parsing.parse_array(row) for row in config["base_path"]]
+            )
             if base_path.shape[1] != 3:
                 raise ValueError(
                     f"base_path must be SE2 [x, y, yaw], got shape {base_path.shape}"
@@ -318,7 +350,8 @@ def __init__(self, config):
 
         # Parse EE path (optional)
         if "ee_path" in config:
-            ee_path = np.array(config["ee_path"])
+            # Parse each row to handle pi notation
+            ee_path = np.array([parsing.parse_array(row) for row in config["ee_path"]])
             if ee_path.shape[1] != 6:
                 raise ValueError(
                     f"ee_path must be SE3 [x, y, z, roll, pitch, yaw], got shape {ee_path.shape}"
@@ -335,7 +368,7 @@ def __init__(self, config):
             )
 
         # Common parameters
-        self.tracking_err_tol = config.get("tracking_err_tol", 0.02)
+        self.tracking_pos_err_tol = config.get("tracking_pos_err_tol", 0.02)
         self.tracking_ori_err_tol = config.get("tracking_ori_err_tol", 0.1)
         self.end_stop = config.get("end_stop", False)
 
@@ -472,7 +505,7 @@ def checkFinished(self, t, states):
             base_vel = states["base"].get("velocity")
             end_pose = self.base_plan["p"][-1]
             pos_err, ori_err = self._compute_error(base_pose, end_pose, is_base=True)
-            pos_cond = pos_err < self.tracking_err_tol
+            pos_cond = pos_err < self.tracking_pos_err_tol
             ori_cond = ori_err < self.tracking_ori_err_tol
             pos_ori_cond = pos_cond and ori_cond
             vel_cond = base_vel is not None and np.linalg.norm(base_vel) < 1e-2
@@ -490,7 +523,7 @@ def checkFinished(self, t, states):
             ee_vel = states["EE"].get("velocity")
             end_pose = self.ee_plan["p"][-1]
             pos_err, ori_err = self._compute_error(ee_pose, end_pose, is_base=False)
-            pos_cond = pos_err < self.tracking_err_tol
+            pos_cond = pos_err < self.tracking_pos_err_tol
             ori_cond = ori_err < self.tracking_ori_err_tol
             pos_ori_cond = pos_cond and ori_cond
             vel_cond = ee_vel is not None and np.linalg.norm(ee_vel) < 1e-2

mm_plan/src/mm_plan/TaskManager.py

@@ -156,8 +156,13 @@ def update(self, t, states):
                     "All tasks completed (%d/%d)", self.planner_num, self.planner_num
                 )
             return True, 1
-
-        return False, 1
+        else:
+            self.logger.info(
+                "Working on task %d/%d",
+                self.curr_task_id + 1,
+                self.planner_num,
+            )
+            return False, 1
 
     def print(self):
         """Print task manager status."""

mm_run/config/3d_collision.yaml

@@ -5,7 +5,7 @@ planner:
     planner_type: "WaypointPlanner"
     ee_pose: [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]  # [x, y, z, roll, pitch, yaw]
     hold_period: 3.0
-    tracking_err_tol: 0.02
+    tracking_pos_err_tol: 0.02
 
 simulation:
   robot:

mm_run/config/configuration.md

@@ -36,7 +36,7 @@ planner:
   tasks:
     - name: str                    # Task identifier
       planner_type: "WaypointPlanner" | "PathPlanner"
-      tracking_err_tol: float      # Position error tolerance [m]
+      tracking_pos_err_tol: float      # Position error tolerance [m]
       tracking_ori_err_tol: float  # Optional: orientation error tolerance [rad] (default: 0.1)
 ```
 
@@ -47,20 +47,20 @@ planner:
   planner_type: "WaypointPlanner"
   base_pose: [x, y, yaw]                    # Optional: SE2 [m, m, rad] in world frame
   ee_pose: [x, y, z, roll, pitch, yaw]      # Optional: SE3 [m, m, m, rad, rad, rad] in world frame
-  tracking_err_tol: 0.1
+  tracking_pos_err_tol: 0.1
   hold_period: 1.0                          # Optional: hold time at target [s]
 
 # Sequential: Only base moves
 - name: "Move Base"
   planner_type: "WaypointPlanner"
   base_pose: [2.0, 1.0, 0.0]
-  tracking_err_tol: 0.2
+  tracking_pos_err_tol: 0.2
 
 # Sequential: Only EE moves
 - name: "Reach Down"
   planner_type: "WaypointPlanner"
   ee_pose: [2.2, 1.2, 0.5, 1.57, 0, 0]
-  tracking_err_tol: 0.05
+  tracking_pos_err_tol: 0.05
   hold_period: 1.0
 ```
 
@@ -72,22 +72,22 @@ planner:
   base_path: [[x1, y1, yaw1], [x2, y2, yaw2], ...]  # Optional: Array of SE2 poses
   ee_path: [[x1, y1, z1, r1, p1, y1], [x2, y2, z2, r2, p2, y2], ...]  # Optional: Array of SE3 poses
   dt: 0.1                         # Time step between path points [s]
-  tracking_err_tol: 0.1
+  tracking_pos_err_tol: 0.1
   end_stop: false                 # Optional: require zero velocity at end (default: false)
 
 # Sequential: Only base path
 - name: "Base Path"
   planner_type: "PathPlanner"
   base_path: [[0, 0, 0], [1, 0, 0], [2, 1, 1.57]]
   dt: 0.1
-  tracking_err_tol: 0.2
+  tracking_pos_err_tol: 0.2
 
 # Sequential: Only EE path
 - name: "EE Path"
   planner_type: "PathPlanner"
   ee_path: [[1, 0, 1, 0, 0, 0], [1.5, 0.2, 0.8, 0, 0, 0]]
   dt: 0.1
-  tracking_err_tol: 0.02
+  tracking_pos_err_tol: 0.02
 ```
 
 **Note**: At least one of `base_pose`/`base_path` or `ee_pose`/`ee_path` must be specified. Tasks execute sequentially, but each task can coordinate base and EE simultaneously.

mm_run/config/simple_experiment.yaml

@@ -11,14 +11,14 @@ planner:
   - name: Base Waypoint
     planner_type: WaypointPlanner
     base_pose: [3.5, 0.4, 3.14]   # x, y, yaw in world frame
-    tracking_err_tol: 0.2
+    tracking_pos_err_tol: 0.2
 
   # Task 2: Move end-effector to target pose (SE3: x, y, z, roll, pitch, yaw)
   - name: EE Waypoint
     planner_type: WaypointPlanner
     ee_pose: [1.5, 0.4, 1.0, 3.14, 0, 0]  # x, y, z, roll, pitch, yaw - EE pointing down
     hold_period: 1.0
-    tracking_err_tol: 0.05
+    tracking_pos_err_tol: 0.05
 
 controller:
   # Enable self-collision avoidance (prevents robot from colliding with itself)

mm_run/config/test_experiments/experiments.md

@@ -1,5 +1,5 @@
 # Integration Test: Complex Multi-Task Sequence
-# Tests: Sequential base + EE tasks with consistent state dictionary format
+# Tests: Sequential base + EE waypoints
 # This verifies that TaskManager correctly sequences multiple tasks
 # and that state conversion (pose + velocity) works throughout
 
@@ -9,13 +9,13 @@ planner:
     planner_type: WaypointPlanner
     base_pose: [0.5, 0.0, 0]  # World frame: [x, y, yaw]
     ee_pose: [1.2, 0.2, 0.9, 0, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
-    tracking_err_tol: 0.5
+    tracking_pos_err_tol: 0.5
     tracking_ori_err_tol: 0.5
   - name: Move 2
     planner_type: WaypointPlanner
     base_pose: [2.0, 0.5, 1.57]  # World frame: [x, y, yaw]
     ee_pose: [2.2, 0.7, 1.0, 1.57, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
-    tracking_err_tol: 0.2
+    tracking_pos_err_tol: 0.2
 
 controller:
   self_collision_avoidance_enabled: True

…t_experiments/test_complex_sequence.yaml …_experiments/test_complex_waypoints.yamlmm_run/config/test_experiments/test_complex_sequence.yaml renamed to mm_run/config/test_experiments/test_complex_waypoints.yaml mm_run/config/test_experiments/test_complex_sequence.yaml renamed to mm_run/config/test_experiments/test_complex_waypoints.yaml

@@ -5,23 +5,29 @@
 
 planner:
   tasks:
-  - name: EE Orientation Change
+  - name: EE Orientation Facing Down
     planner_type: WaypointPlanner
-    ee_pose: [1.0, 0.0, 0.8, 0, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
-    tracking_err_tol: 0.05
-  - name: EE Rotate 180
+    ee_pose: [3, 0, 0, -1pi, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
+    ee_mask: [false, false, false, true, true, true]  # Only orientation matters
+    tracking_ori_err_tol: 0.05
+  - name: EE Orientation Facing Backwards
     planner_type: WaypointPlanner
-    ee_pose: [1.0, 0.0, 0.8, 3.14, 0, 0]  # Same position, rotate 180° around x-axis (world frame)
-    tracking_err_tol: 0.05
+    ee_pose: [0, 2, 0, -1pi, 0.5pi, 0]  # World frame: [x, y, z, roll, pitch, yaw]
+    ee_mask: [false, false, false, true, true, true]  # Only orientation matters
+    tracking_ori_err_tol: 0.05
 
 controller:
   self_collision_avoidance_enabled: True
   static_obstacles_collision_avoidance_enabled: False
   acados:
     name: "TestOrientationTracking"
+  cost_params:
+    EEPose:
+      Qk: [0, 0, 0, 3, 3, 3]  # [x, y, z, roll, pitch, yaw]
+      P: [0, 0, 0, 1, 1, 1]   # Terminal weights
 
 simulation:
-  duration: 8
+  duration: 6
 
 include:
   - package: "mm_run"