From f5641368760f7b682c96d8437aded8baeb5d9c33 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 08:05:15 +0800 Subject: [PATCH 1/4] feat: add per_object_yaw_tolerance to NavigateSkill Allow different yaw_tolerance per target object in moveto skill, similar to per_object_sampling_radius. Also add timeout position logging in pipeline when max_steps is reached. Co-Authored-By: Claude Opus 4.6 --- source/autosim/autosim/core/pipeline.py | 13 +++++++++++++ source/autosim/autosim/skills/navigate.py | 12 ++++++++++++ 2 files changed, 25 insertions(+) diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index c81475e..a29a809 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -162,6 +162,7 @@ def execute_skill_sequence(self, decompose_result: DecomposeResult): self._logger.info(f"Skill {skill_info.skill_type} skipped due to action adapter setting.") continue + self._action_adapter.pre_skill_hook(skill_info.skill_type, self._env, self._last_action) goal = skill.extract_goal_from_info(skill_info, self._env, self._env_extra_info) success, steps = self._execute_single_skill(skill, goal) @@ -216,6 +217,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 hasattr(goal, "target_pose") and 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: diff --git a/source/autosim/autosim/skills/navigate.py b/source/autosim/autosim/skills/navigate.py index 6f357af..1d45003 100644 --- a/source/autosim/autosim/skills/navigate.py +++ b/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]}") From d3f6ffc25411151b67aa962480fd474a2b0716b0 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 11:55:30 +0800 Subject: [PATCH 2/4] delete extra func --- source/autosim/autosim/core/pipeline.py | 1 - 1 file changed, 1 deletion(-) diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index a29a809..db8643a 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -162,7 +162,6 @@ def execute_skill_sequence(self, decompose_result: DecomposeResult): self._logger.info(f"Skill {skill_info.skill_type} skipped due to action adapter setting.") continue - self._action_adapter.pre_skill_hook(skill_info.skill_type, self._env, self._last_action) goal = skill.extract_goal_from_info(skill_info, self._env, self._env_extra_info) success, steps = self._execute_single_skill(skill, goal) From 203db6a0550cf1dde925235542e7674fdd5eb88a Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 14:55:01 +0800 Subject: [PATCH 3/4] fix --- source/autosim/autosim/core/pipeline.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index db8643a..5734c5e 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -220,13 +220,12 @@ def _execute_single_skill(self, skill: Skill, goal: SkillGoal) -> tuple[bool, in if steps >= self.cfg.max_steps: world_state = self._build_world_state() current_pos = world_state.robot_base_pose[:2] - if hasattr(goal, "target_pose") and 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" - ) + 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 From daded8754e4f4301fb76dafa3245a272240e5cf5 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 14:57:41 +0800 Subject: [PATCH 4/4] fix --- source/autosim/autosim/core/pipeline.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index 5734c5e..f432ae1 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -220,12 +220,13 @@ def _execute_single_skill(self, skill: Skill, goal: SkillGoal) -> tuple[bool, in if steps >= self.cfg.max_steps: world_state = self._build_world_state() current_pos = world_state.robot_base_pose[:2] - 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" - ) + 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