From b302669544b557a31648d97c030bcfd18727e230 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 31 Aug 2026 11:15:32 +0000 Subject: [PATCH] fix(tutorials): align UR solver joint limits Configure atomic-action UR tutorial solvers with DexSim's runtime +/-2*pi limits and preserve them for dual-arm PyTorch solvers. Add regression coverage for the no-op limit-sync warning. --- .../tutorials/atomic_action/scenario_utils.py | 1 + .../tutorials/atomic_action/tutorial_utils.py | 16 +++++- .../sim/atomic_actions/test_tutorial_utils.py | 53 +++++++++++++++++++ 3 files changed, 68 insertions(+), 2 deletions(-) diff --git a/scripts/tutorials/atomic_action/scenario_utils.py b/scripts/tutorials/atomic_action/scenario_utils.py index 055947a0c..a44c6ac52 100644 --- a/scripts/tutorials/atomic_action/scenario_utils.py +++ b/scripts/tutorials/atomic_action/scenario_utils.py @@ -150,6 +150,7 @@ def create_dual_tutorial_robot_cfg( root_link_name=base_solver.root_link_name, tcp=tcp, num_samples=pytorch_num_samples, + user_qpos_limits=base_solver.user_qpos_limits, ) base_cfg.solver_cfg["arm"] = base_solver diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 7171c7d21..eac646336 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -78,6 +78,12 @@ (0.0, 0.0, 1.0, 0.21), (0.0, 0.0, 0.0, 1.0), ) +# DexSim canonicalizes the UR source URDF's rounded +/-6.2832 limits to +/-2*pi. +# Configure the solver with that physical range up front to avoid a no-op limit-sync +# warning during tutorial robot construction. +_UR_TUTORIAL_QPOS_LIMITS: tuple[tuple[float, float], ...] = tuple( + (-2.0 * math.pi, 2.0 * math.pi) for _ in range(6) +) TUTORIAL_PARALLEL_JAW_MODEL = ParallelJawGripperModelCfg( model_id="dh_pgi_140_80", min_opening_width=0.003, @@ -934,7 +940,8 @@ def create_ur5_gripper_robot_cfg( [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, tcp_z], [0.0, 0.0, 0.0, 1.0], - ] + ], + "user_qpos_limits": _UR_TUTORIAL_QPOS_LIMITS, } }, "init_qpos": qpos, @@ -1056,7 +1063,12 @@ def create_ur10_robotiq_robot_cfg( "damping": {ROBOTIQ_HAND_JOINT_PATTERN: 1e2}, "max_effort": {ROBOTIQ_HAND_JOINT_PATTERN: 1e3}, }, - "solver_cfg": {"arm": {"tcp": ROBOTIQ_2F_140_TCP}}, + "solver_cfg": { + "arm": { + "tcp": ROBOTIQ_2F_140_TCP, + "user_qpos_limits": _UR_TUTORIAL_QPOS_LIMITS, + } + }, "init_qpos": qpos, "init_pos": init_pos, } diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 131518d77..73ec4d748 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -31,6 +31,7 @@ import torch from embodichain.lab.sim.atomic_actions import TimedTrajectory +from embodichain.lab.sim.solvers import BaseSolver from scripts.tutorials.atomic_action.dynamic_obstacle_recovery import ( _animate_obstacle_to_pose, _blocking_obstacle_pose, @@ -74,6 +75,7 @@ STRICT_RECOVERY_MINIMUM_CLEARANCE = 0.01 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) PGI_TUTORIAL_TCP = torch.tensor( [ [1.0, 0.0, 0.0, 0.0], @@ -343,6 +345,37 @@ def test_ur10_robotiq_config_matches_six_active_hand_joints_and_tcp() -> None: ) +@pytest.mark.parametrize( + "factory", + (create_ur5_gripper_robot_cfg, create_ur10_robotiq_robot_cfg), +) +def test_ur_tutorial_solvers_use_runtime_joint_limits(factory) -> None: + cfg = factory() + + assert torch.allclose( + torch.as_tensor(cfg.solver_cfg["arm"].user_qpos_limits), + UR_RUNTIME_QPOS_LIMITS, + ) + + +@pytest.mark.parametrize( + "factory", + (create_ur5_gripper_robot_cfg, create_ur10_robotiq_robot_cfg), +) +def test_ur_tutorial_solver_limits_skip_noop_hard_limit_warning(factory) -> None: + cfg = factory() + solver_limits = torch.as_tensor(cfg.solver_cfg["arm"].user_qpos_limits) + solver = SimpleNamespace( + lower_qpos_limits=solver_limits[:, 0].clone(), + upper_qpos_limits=solver_limits[:, 1].clone(), + ) + + with patch("embodichain.lab.sim.solvers.base_solver.logger.log_warning") as warning: + BaseSolver.update_with_robot_limit(solver, UR_RUNTIME_QPOS_LIMITS) + + assert not warning.called + + @pytest.mark.parametrize( ("robot_type", "arm_dof", "solver_name", "hand_pattern", "expected_tcp"), ( @@ -395,12 +428,32 @@ def test_dual_tutorial_configs_share_hand_binding_contract( solver = dual_cfg.solver_cfg[f"{side}_arm"] assert type(solver).__name__ == solver_name assert torch.allclose(torch.as_tensor(solver.tcp), expected_tcp_tensor) + assert solver.user_qpos_limits == single_cfg.solver_cfg["arm"].user_qpos_limits if robot_type == "ur10": assert len(dual_cfg.init_qpos) == 24 assert dual_cfg.init_qpos[-12:] == [0.0] * 12 +@pytest.mark.parametrize("robot_type", ("ur5", "ur10")) +def test_dual_pytorch_ur_tutorial_solvers_preserve_runtime_joint_limits( + robot_type: str, +) -> None: + cfg = create_dual_tutorial_robot_cfg( + robot_type=robot_type, + uid=f"test_{robot_type}_pytorch", + urdf_name=f"test_dual_{robot_type}_pytorch", + tcp_z=0.121, + solver="pytorch", + ) + + for side in ("left", "right"): + assert torch.allclose( + torch.as_tensor(cfg.solver_cfg[f"{side}_arm"].user_qpos_limits), + UR_RUNTIME_QPOS_LIMITS, + ) + + def test_dual_franka_mount_preserves_single_arm_facing_direction() -> None: cfg = create_dual_tutorial_robot_cfg( robot_type="franka",