Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions scripts/tutorials/atomic_action/scenario_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
16 changes: 14 additions & 2 deletions scripts/tutorials/atomic_action/tutorial_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
}
Expand Down
53 changes: 53 additions & 0 deletions tests/sim/atomic_actions/test_tutorial_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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],
Expand Down Expand Up @@ -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"),
(
Expand Down Expand Up @@ -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",
Expand Down
Loading