YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add URDF model for RM75-B OmniPicker with detailed link and joint specifications

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-28 17:06:06 +08:00
parent fae5a560fb
commit 2a12eea4d5
34 changed files with 2006 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_orientation_control.py
+96 -31
View File
@@ -2,14 +2,19 @@ import math
import time
from types import SimpleNamespace
import numpy as np
import pytest
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
from xr_rm_teleop.realman_adapter import JointStateSnapshot
from xr_rm_teleop.single_arm_velocity_teleop import (
SingleArmVelocityTeleop,
_euler_to_quaternion,
_make_transform,
_matrix_to_quaternion,
_normalize_quaternion,
_quaternion_to_euler,
_project_rotation,
_quaternion_to_matrix,
_so3_exp,
_so3_log,
)
@@ -18,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
teleop._enable_orientation_control = True
teleop._enable_orientation_axes = [True, True, True]
teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
teleop._robot_start_transform = _make_transform(
[0.3, 0.0, 0.2],
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
)
teleop._xr_to_robot_matrix = [
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
@@ -27,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
return teleop
def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
assert len(actual) == len(expected)
for actual_value, expected_value in zip(actual, expected):
assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
teleop = _make_teleop_for_orientation()
target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
assert_angles_close(target, teleop._robot_start_pose.rpy())
assert target == pytest.approx(teleop._robot_start_transform[:3, :3])
def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
teleop = _make_teleop_for_orientation()
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
teleop._robot_start_transform = np.eye(4)
xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0])))
target = teleop._raw_orientation_from_controller(xr_roll)
assert_angles_close(target, [0.0, 0.0, 0.2])
assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2])
def test_orientation_deadband_filter_and_speed_limit() -> None:
def test_quaternion_sign_does_not_change_rotation() -> None:
quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9))
assert _quaternion_to_matrix(quaternion) == pytest.approx(
_quaternion_to_matrix(tuple(-value for value in quaternion))
)
@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5])
def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None:
teleop = _make_teleop_for_orientation()
start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0]))
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0])))
target = teleop._raw_orientation_from_controller(controller)
error = _so3_log(target @ start_rotation.T)
assert np.linalg.norm(error) == pytest.approx(0.01)
def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None:
teleop = _make_teleop_for_orientation()
start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0]))
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0])))
target = teleop._raw_orientation_from_controller(controller)
assert _so3_log(target @ start_rotation.T) == pytest.approx(
[0.0, 0.002, 0.0],
abs=1e-9,
)
def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None:
teleop = _make_teleop_for_orientation()
teleop._robot_start_transform = np.eye(4)
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
teleop._enable_orientation_axes = [True, False, True]
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3])))
target = teleop._raw_orientation_from_controller(controller)
assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3])
def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None:
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._orientation_deadband_rad = 0.01
teleop._orientation_filter_alpha = 0.5
teleop._max_orientation_speed = 0.5
teleop._dt = 0.1
teleop._last_sent_orientation = [0.0, 0.0, 0.0]
teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
teleop._dt = 1.0 / 125.0
teleop._last_sent_orientation = np.eye(3)
teleop._filtered_orientation_target = np.eye(3)
assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0]))
assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3))
filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
assert_angles_close(filtered, [0.1, 0.0, 0.0])
target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
filtered = teleop._filter_orientation_target(target)
assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0])
limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
limited, was_limited = teleop._limit_orientation_step(target)
assert was_limited
assert_angles_close(limited, [0.05, 0.0, 0.0])
assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0)
def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None:
quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3])))
assert np.isfinite(quaternion).all()
assert np.linalg.norm(quaternion) == pytest.approx(1.0)
def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None:
near_rotation = np.eye(3)
near_rotation[0, 1] = 1e-5
projected = _project_rotation(near_rotation)
assert projected.T @ projected == pytest.approx(np.eye(3))
assert np.linalg.det(projected) == pytest.approx(1.0)
with pytest.raises(ValueError):
_project_rotation(np.diag([2.0, 1.0, 1.0]))
def test_invalid_controller_quaternion_stops_current_tick() -> None:
@@ -102,9 +174,7 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
time.monotonic(),
)
)
teleop._ik_solver = SimpleNamespace(
update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
)
teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
teleop._active = False
teleop._last_valid_joint_target = None
teleop._last_current_pose = None
@@ -119,11 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
assert stopped == [True]
def test_quaternion_roundtrip_for_small_rpy() -> None:
quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
def test_zero_quaternion_is_invalid() -> None:
with pytest.raises(ValueError):
_normalize_quaternion([0.0, 0.0, 0.0, 0.0])