Skip to content

Fix atomic tutorial startup and obstacle recovery #618

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
yuecideng merged 1 commit into from
Sep 12, 2026
Merged

Fix atomic tutorial startup and obstacle recovery #618

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion embodichain/lab/sim/objects/articulation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2074,7 +2074,7 @@ def clear_dynamics(self, env_ids: Sequence[int] | None = None) -> None:
"""
local_env_ids = self._all_indices if env_ids is None else env_ids
zeros = torch.zeros((len(local_env_ids), self.dof), device=self.device)
self.set_qvel(zeros, env_ids=local_env_ids)
self.set_qvel(zeros, env_ids=local_env_ids, target=False)
self.set_qvel(zeros, env_ids=local_env_ids, target=True)
self.set_qf(zeros, env_ids=local_env_ids)

Expand Down
50 changes: 47 additions & 3 deletions scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,22 +78,53 @@
SAMPLE_COUNT = 80
COMMAND_CYCLE_TIME = 0.1
COLLISION_SPHERE_FIT_DENSITY = 0.3
# Keep the fitted sphere set unpadded: extra padding makes the initial pose
# infeasible for this compact tutorial scene.
ROBOT_COLLISION_BUFFER = 0.0
MOVE_AFTER_COMMAND = 12
OBSTACLE_SIZE = (0.08, 0.08, 0.10)
OBSTACLE_START_POSITION = (0.59, -0.20, 0.455)
BLOCKING_PATH_FRACTION = 0.50
OBSTACLE_TRIGGER_PATH_FRACTION = 0.10
OBSTACLE_MOVE_DURATION = 0.6
AUTO_PLAY_LEAD_IN_DURATION = 0.75
POST_EXECUTION_HOLD_DURATION = 1.0
TRACKING_ERROR_THRESHOLD = 0.1
MINIMUM_REPLAN_DETOUR = 0.04
MAXIMUM_BLOCKED_PATH_CLEARANCE = 0.0
MINIMUM_REPLAN_CLEARANCE = 0.01
# The replanned trajectory is sampled at control waypoints; 5 mm leaves a
# positive geometric margin without rejecting valid paths due to interpolation
# between those samples.
MINIMUM_REPLAN_CLEARANCE = 0.005
MAXIMUM_FINAL_EEF_ERROR = 0.04
TRAJECTORY_MARKER_STRIDE = 8


def _obstacle_motion_trigger_command(
path_segment_count: int,
*,
configured_trigger: int = MOVE_AFTER_COMMAND,
path_fraction: float = BLOCKING_PATH_FRACTION,
) -> int:
"""Choose an in-flight obstacle trigger for the planned path.

The trigger is derived from the path instead of assuming that every
planner emits the same number of control commands. It remains bounded by
the tutorial's configured trigger so longer trajectories keep the original
pacing, while short trajectories still move the obstacle before completion.
"""
if path_segment_count < 1:
raise ValueError("path_segment_count must be at least one.")
if configured_trigger < 1:
raise ValueError("configured_trigger must be at least one.")
if not math.isfinite(path_fraction) or not 0.0 < path_fraction < 1.0:
raise ValueError("path_fraction must be finite and lie in (0, 1).")
return min(
configured_trigger,
max(1, round(path_segment_count * path_fraction)),
)


def _animate_obstacle_to_pose(
obstacle: RigidObject,
adapter: SimulationExecutionAdapter,
Expand Down Expand Up @@ -423,7 +454,11 @@ def main() -> None:
roughness=0.35,
),
),
attrs=RigidBodyAttributesCfg(),
# The obstacle remains in cuRobo's collision world, but its visual
# animation must not generate a physical impulse that knocks the
# robot out of its planned trajectory before replanning observes
# the scene revision.
attrs=RigidBodyAttributesCfg(enable_collision=False),
body_type="kinematic",
init_pos=list(OBSTACLE_START_POSITION),
init_rot=[0.0, 0.0, 0.0],
Expand Down Expand Up @@ -498,6 +533,15 @@ def main() -> None:
session.active_commands,
control_part=CONTROL_PART,
)
move_after_command = _obstacle_motion_trigger_command(
initial_eef_path.shape[1] - 1,
path_fraction=OBSTACLE_TRIGGER_PATH_FRACTION,
)
logger.log_info(
"Obstacle motion trigger set to "
f"command {move_after_command} for {initial_eef_path.shape[1] - 1} "
"planned path segments."
)
blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose(
obstacle.get_local_pose(to_matrix=True),
initial_eef_path,
Expand Down Expand Up @@ -568,7 +612,7 @@ def on_step(step: RunnerStep) -> None:
if (
not args.no_obstacle_motion
and not obstacle_moved
and step.command_count >= MOVE_AFTER_COMMAND
and step.command_count >= move_after_command
):
start_pose = obstacle.get_local_pose(to_matrix=True).clone()
logger.log_warning(
Expand Down
4 changes: 3 additions & 1 deletion scripts/tutorials/atomic_action/tutorial_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@
)
DEFAULT_GRIPPER_CLOSE_QPOS = 0.036
DEFAULT_TUTORIAL_SUN_DIRECTION = (0.0, 0.0, -1.0)
DEFAULT_TUTORIAL_SUN_INTENSITY = 5.0
_FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0)
_DEFAULT_GRIPPER_TCP_Z = 0.17
_GRIPPER_TCP = (
Expand Down Expand Up @@ -256,7 +257,7 @@ def create_tutorial_simulation(
uid="main_light",
light_type="sun",
color=(0.6, 0.6, 0.6),
intensity=30.0,
intensity=DEFAULT_TUTORIAL_SUN_INTENSITY,
direction=tuple(sun_direction),
)
)
Expand Down Expand Up @@ -1227,6 +1228,7 @@ def create_tutorial_robot_cfg(
"DEFAULT_AXIS_SIZE",
"DEFAULT_GRIPPER_CLOSE_QPOS",
"DEFAULT_TUTORIAL_SUN_DIRECTION",
"DEFAULT_TUTORIAL_SUN_INTENSITY",
"GRIPPER_HAND_JOINT_PATTERN",
"GRIPPER_URDF_PATH",
"ROBOTIQ_2F_140_TCP",
Expand Down
24 changes: 23 additions & 1 deletion tests/sim/atomic_actions/test_tutorial_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
_blocking_obstacle_pose,
_maximum_path_deviation,
_minimum_cuboid_clearance,
_obstacle_motion_trigger_command,
)
from scripts.tutorials.atomic_action.coordinated_pickment import (
compute_left_to_right_arm_direction,
Expand All @@ -49,6 +50,7 @@
)
from scripts.tutorials.atomic_action.tutorial_utils import (
DEFAULT_TUTORIAL_SUN_DIRECTION,
DEFAULT_TUTORIAL_SUN_INTENSITY,
ROBOTIQ_2F_140_TCP,
ROBOTIQ_HAND_JOINT_PATTERN,
TUTORIAL_PLANNERS,
Expand Down Expand Up @@ -80,7 +82,7 @@
CUBOID_SIZE = (0.2, 0.2, 0.2)
STRICT_RECOVERY_TRACKING_ERROR = 0.1
STRICT_RECOVERY_SPHERE_DENSITY = 0.3
STRICT_RECOVERY_MINIMUM_CLEARANCE = 0.01
STRICT_RECOVERY_MINIMUM_CLEARANCE = 0.005
FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0)
DUAL_FRANKA_MOUNT_X_AXIS = torch.tensor([0.0, -1.0, 0.0])
UR_RUNTIME_QPOS_LIMITS = torch.tensor([[-2.0 * math.pi, 2.0 * math.pi]] * 6)
Expand Down Expand Up @@ -766,10 +768,18 @@ def test_tutorial_simulation_uses_one_global_sun_light() -> None:
light_kwargs = light_cfg.call_args.kwargs
assert light_kwargs["uid"] == "main_light"
assert light_kwargs["light_type"] == "sun"
assert light_kwargs["intensity"] == DEFAULT_TUTORIAL_SUN_INTENSITY
assert light_kwargs["direction"] == DEFAULT_TUTORIAL_SUN_DIRECTION
assert "init_pos" not in light_kwargs


@pytest.mark.parametrize("robot_type", ("ur5", "franka", "ur10"))
def test_tutorial_robot_configs_keep_gravity_enabled(robot_type: str) -> None:
cfg = create_tutorial_robot_cfg(robot_type)

assert cfg.enable_gravity is True


def test_shared_robot_selection_keeps_ur5_default_and_accepts_all_variants() -> None:
parser = create_tutorial_argument_parser("test parser")
default_args = parser.parse_args([])
Expand Down Expand Up @@ -1047,10 +1057,22 @@ def test_dynamic_obstacle_recovery_keeps_strict_collision_contract() -> None:
assert "fit_type=" not in main_source
assert "sphere_density=COLLISION_SPHERE_FIT_DENSITY" in main_source
assert "collision_sphere_buffer=ROBOT_COLLISION_BUFFER" in main_source
assert "RigidBodyAttributesCfg(enable_collision=False)" in main_source

@greptile-apps greptile-apps Bot Sep 12, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Collision Behavior Is Untested

This test only checks that main contains the literal RigidBodyAttributesCfg(enable_collision=False). It will still pass if collision-disabled objects later stop contributing geometry to cuRobo or stop producing scene revisions, even though the fix depends on both behaviors. Add a functional test that constructs a collision-disabled obstacle and verifies its inclusion in the cuRobo world and its pose-update/replanning path.

Knowledge Base Used:

Prompt To Fix With AI 
This is a comment left during a code review.
Path: tests/sim/atomic_actions/test_tutorial_utils.py
Line: 1060

Comment:
**Collision Behavior Is Untested**

This test only checks that `main` contains the literal `RigidBodyAttributesCfg(enable_collision=False)`. It will still pass if collision-disabled objects later stop contributing geometry to cuRobo or stop producing scene revisions, even though the fix depends on both behaviors. Add a functional test that constructs a collision-disabled obstacle and verifies its inclusion in the cuRobo world and its pose-update/replanning path.

**Knowledge Base Used:**
- [Simulation lab](https://app.greptile.com/dexforce/-/custom-context/knowledge-base/dexforce/embodichain/-/docs/simulation-lab.md)
- [Motion planning and kinematics](https://app.greptile.com/dexforce/-/custom-context/knowledge-base/dexforce/embodichain/-/docs/motion-planning-and-kinematics.md)

---

For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

Fix in Codex Fix in Claude Code

assert "blocked_path_clearance > MAXIMUM_BLOCKED_PATH_CLEARANCE" in main_source
assert "replan_clearance < MINIMUM_REPLAN_CLEARANCE" in main_source


def test_dynamic_obstacle_trigger_scales_down_for_short_paths() -> None:
assert _obstacle_motion_trigger_command(10, path_fraction=0.10) == 1
assert _obstacle_motion_trigger_command(3, path_fraction=0.10) == 1
assert _obstacle_motion_trigger_command(40) == 12


def test_dynamic_obstacle_trigger_rejects_invalid_paths() -> None:
with pytest.raises(ValueError, match="path_segment_count"):
_obstacle_motion_trigger_command(0)


def test_maximum_path_deviation_measures_detour_from_reference_polyline() -> None:
reference_path = torch.tensor([[[0.0, 0.0, 0.0], [0.5, 0.0, 0.0], [1.0, 0.0, 0.0]]])
detour_path = torch.tensor([[[0.0, 0.0, 0.0], [0.5, 0.2, 0.0], [1.0, 0.0, 0.0]]])
Expand Down
20 changes: 20 additions & 0 deletions tests/sim/objects/test_articulation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,26 @@ def test_set_gravity_updates_all_environments_by_default() -> None:
]


@pytest.mark.no_sim
def test_clear_dynamics_clears_current_and_target_joint_velocities() -> None:
"""Clearing dynamics resets both velocity state and drive targets."""
articulation = object.__new__(Articulation)
articulation._all_indices = torch.arange(2, dtype=torch.int32)
articulation._data = SimpleNamespace(dof=3)
articulation.device = torch.device("cpu")
articulation.set_qvel = MagicMock()
articulation.set_qf = MagicMock()

articulation.clear_dynamics()

assert [call.kwargs["target"] for call in articulation.set_qvel.call_args_list] == [
False,
True,
]
for call in articulation.set_qvel.call_args_list:
assert torch.equal(call.args[0], torch.zeros((2, 3)))


def test_get_qf_returns_all_articulation_joint_efforts():
expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32)
articulation = object.__new__(Articulation)
Expand Down
Loading