feat: add per_object_yaw_tolerance to NavigateSkill

source/autosim/autosim/core/pipeline.py

@@ -216,6 +216,18 @@ def _execute_single_skill(self, skill: Skill, goal: SkillGoal) -> tuple[bool, in
             if output.done:
                 return True, steps
 
+        # Log current and target positions when max_steps reached
+        if steps >= self.cfg.max_steps:
+            world_state = self._build_world_state()
+            current_pos = world_state.robot_base_pose[:2]
+            if goal.target_pose is not None:
+                target_pos = goal.target_pose[:2]
+                dist = float(torch.linalg.norm(current_pos - target_pos))
+                self._logger.warning(
+                    f"Max steps reached. Current pos: ({current_pos[0]:.3f}, {current_pos[1]:.3f}), "
+                    f"Target pos: ({target_pos[0]:.3f}, {target_pos[1]:.3f}), Distance: {dist:.3f}m"
+                )
+
         return False, steps
 
     def _build_world_state(self) -> WorldState:

source/autosim/autosim/skills/navigate.py

@@ -37,6 +37,10 @@ class NavigateSkillExtraCfg(SkillExtraCfg):
     """The tolerance of the yaw (radians)."""
     sampling_radius: float = 0.8
     """The sampling radius for the target position, in meters."""
+    per_object_sampling_radius: dict[str, float] | None = None
+    """Per-object override for sampling_radius. Keys are object names; unmatched objects use sampling_radius."""
+    per_object_yaw_tolerance: dict[str, float] | None = None
+    """Per-object override for yaw_tolerance. Keys are object names; unmatched objects use yaw_tolerance."""
     num_samples: int = 4
     """The number of samples for the target position."""
 
@@ -87,6 +91,14 @@ def extract_goal_from_info(
             raise ValueError(f"Object {target_object_name} not found in scene")
         target_object = env.scene[target_object_name]
 
+        per_obj = self.cfg.extra_cfg.per_object_sampling_radius or {}
+        if target_object_name in per_obj:
+            self.cfg.extra_cfg.sampling_radius = per_obj[target_object_name]
+
+        per_obj_yaw = self.cfg.extra_cfg.per_object_yaw_tolerance or {}
+        if target_object_name in per_obj_yaw:
+            self.cfg.extra_cfg.yaw_tolerance = per_obj_yaw[target_object_name]
+
         obj_pos_w = target_object.data.root_pos_w[0].cpu().numpy()
         self._logger.info(f"Object pose in world frame: {target_object.data.root_pose_w[0]}")