417 lines
13 KiB
Python
417 lines
13 KiB
Python
import math
|
|
from pathlib import Path
|
|
from types import SimpleNamespace
|
|
from xml.etree import ElementTree
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from xr_rm_teleop import placo_ik_solver
|
|
from xr_rm_teleop.placo_ik_solver import (
|
|
QP_ORIENTATION_TOLERANCE_RAD,
|
|
QP_POSITION_TOLERANCE_M,
|
|
PlacoIkSolver,
|
|
_validated_transform,
|
|
)
|
|
|
|
DUAL_URDF_PATH = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "models"
|
|
/ "dual_rm75"
|
|
/ "Dual_arm.urdf"
|
|
)
|
|
ARM_CASES = (
|
|
(
|
|
"left",
|
|
[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
|
|
list(range(14, 21)),
|
|
list(range(13, 20)),
|
|
"omnipic",
|
|
"scissor_base_link",
|
|
"scissor_scissor_tcp",
|
|
),
|
|
(
|
|
"right",
|
|
[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
|
|
list(range(7, 14)),
|
|
list(range(6, 13)),
|
|
"scissor",
|
|
"omnipic_base_link",
|
|
"omnipic_OmniPic_tcp",
|
|
),
|
|
)
|
|
|
|
|
|
def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> None:
|
|
root = ElementTree.parse(DUAL_URDF_PATH).getroot()
|
|
moving_joint_names = [
|
|
joint.attrib["name"]
|
|
for joint in root.findall("joint")
|
|
if joint.attrib["type"] != "fixed"
|
|
]
|
|
mesh_filenames = [
|
|
mesh.attrib["filename"]
|
|
for mesh in root.findall(".//mesh")
|
|
]
|
|
fixed_joints = {
|
|
"omnipic_base_mount_joint": (
|
|
"dual_arm_base_link",
|
|
"omnipic_base_link",
|
|
None,
|
|
),
|
|
"scissor_base_mount_joint": (
|
|
"dual_arm_base_link",
|
|
"scissor_base_link",
|
|
None,
|
|
),
|
|
"omnipic_OmniPic_tcp_fixed": (
|
|
"omnipic_gripper_link",
|
|
"omnipic_OmniPic_tcp",
|
|
"0 0 0.14",
|
|
),
|
|
"scissor_scissor_tcp_fixed": (
|
|
"scissor_scissor_link",
|
|
"scissor_scissor_tcp",
|
|
"0 0 0",
|
|
),
|
|
"scissor_scissor_fixed_joint": (
|
|
"scissor_link_7",
|
|
"scissor_scissor_link",
|
|
"0 0 0.165",
|
|
),
|
|
}
|
|
|
|
assert moving_joint_names == [
|
|
*[f"omnipic_joint_{index}" for index in range(1, 8)],
|
|
*[f"scissor_joint_{index}" for index in range(1, 8)],
|
|
]
|
|
assert all(filename.startswith("meshes/") for filename in mesh_filenames)
|
|
for name, (parent, child, xyz) in fixed_joints.items():
|
|
joint = root.find(f"joint[@name='{name}']")
|
|
assert joint is not None
|
|
assert joint.attrib["type"] == "fixed"
|
|
assert joint.find("parent").attrib["link"] == parent
|
|
assert joint.find("child").attrib["link"] == child
|
|
if xyz is not None:
|
|
assert joint.find("origin").attrib["xyz"] == xyz
|
|
|
|
|
|
def test_left_scissor_mesh_matches_physical_mount_rotation() -> None:
|
|
root = ElementTree.parse(DUAL_URDF_PATH).getroot()
|
|
link = root.find("link[@name='scissor_scissor_link']")
|
|
|
|
assert link is not None
|
|
assert link.find("visual/origin").attrib["rpy"] == "0 0 -1.5708"
|
|
assert link.find("collision/origin").attrib["rpy"] == "0 0 -1.5708"
|
|
tcp_joint = root.find("joint[@name='scissor_scissor_tcp_fixed']")
|
|
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
|
|
|
|
|
|
def _dual_placo_solver(
|
|
arm: str,
|
|
joint_degrees: list[float],
|
|
) -> tuple[PlacoIkSolver, list[float]]:
|
|
pytest.importorskip("placo")
|
|
joints = [math.radians(value) for value in joint_degrees]
|
|
return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
|
|
"expected_base_frame,expected_tcp_frame",
|
|
ARM_CASES,
|
|
)
|
|
def test_solver_uses_arm_specific_offsets(
|
|
arm: str,
|
|
joint_degrees: list[float],
|
|
q_offsets: list[int],
|
|
v_offsets: list[int],
|
|
inactive_prefix: str,
|
|
expected_base_frame: str,
|
|
expected_tcp_frame: str,
|
|
) -> None:
|
|
solver, _ = _dual_placo_solver(arm, joint_degrees)
|
|
|
|
assert solver._q_offsets.tolist() == q_offsets
|
|
assert solver._v_offsets.tolist() == v_offsets
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
|
|
"expected_base_frame,expected_tcp_frame",
|
|
ARM_CASES,
|
|
)
|
|
def test_joint_state_pose_is_relative_to_selected_arm_base(
|
|
arm: str,
|
|
joint_degrees: list[float],
|
|
q_offsets: list[int],
|
|
v_offsets: list[int],
|
|
inactive_prefix: str,
|
|
expected_base_frame: str,
|
|
expected_tcp_frame: str,
|
|
) -> None:
|
|
solver, joints = _dual_placo_solver(arm, joint_degrees)
|
|
|
|
actual_pose = solver.update_joint_state(joints)
|
|
assert solver._base_frame == expected_base_frame
|
|
assert solver._tcp_frame == expected_tcp_frame
|
|
world_base = solver._robot.get_T_world_frame(expected_base_frame)
|
|
world_tcp = solver._robot.get_T_world_frame(expected_tcp_frame)
|
|
|
|
assert actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
|
|
"expected_base_frame,expected_tcp_frame",
|
|
ARM_CASES,
|
|
)
|
|
def test_qp_solve_converges_without_moving_inactive_arm(
|
|
arm: str,
|
|
joint_degrees: list[float],
|
|
q_offsets: list[int],
|
|
v_offsets: list[int],
|
|
inactive_prefix: str,
|
|
expected_base_frame: str,
|
|
expected_tcp_frame: str,
|
|
) -> None:
|
|
solver, joints = _dual_placo_solver(arm, joint_degrees)
|
|
start_pose = solver.update_joint_state(joints)
|
|
inactive_q_offsets = [
|
|
solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
|
|
for index in range(1, 8)
|
|
]
|
|
inactive_before = solver._robot.state.q[inactive_q_offsets].copy()
|
|
target_pose = start_pose.copy()
|
|
target_pose[0, 3] += 0.01
|
|
|
|
result = solver.solve(target_pose)
|
|
reached_pose = solver.update_joint_state(result)
|
|
position_error = np.linalg.norm(
|
|
target_pose[:3, 3] - reached_pose[:3, 3]
|
|
)
|
|
rotation_delta = (
|
|
target_pose[:3, :3] @ reached_pose[:3, :3].T
|
|
)
|
|
orientation_error = math.acos(
|
|
float(
|
|
np.clip(
|
|
(np.trace(rotation_delta) - 1.0) * 0.5,
|
|
-1.0,
|
|
1.0,
|
|
)
|
|
)
|
|
)
|
|
|
|
assert np.asarray(result).shape == (7,)
|
|
assert np.isfinite(result).all()
|
|
assert position_error <= QP_POSITION_TOLERANCE_M
|
|
assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
|
|
assert solver._robot.state.q[inactive_q_offsets] == pytest.approx(
|
|
inactive_before
|
|
)
|
|
|
|
|
|
def test_solver_rejects_unknown_arm() -> None:
|
|
with pytest.raises(ValueError, match="arm must be left or right"):
|
|
PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
|
|
|
|
|
|
def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
|
|
solver = object.__new__(PlacoIkSolver)
|
|
solver._actual_joints = np.zeros(7)
|
|
solver._q_offsets = np.arange(7, 14)
|
|
solver._robot = SimpleNamespace(
|
|
state=SimpleNamespace(q=np.zeros(21))
|
|
)
|
|
solver._frame_task = SimpleNamespace(T_a_b=None)
|
|
solver._target_errors = lambda: (1.5e-3, 0.0)
|
|
|
|
result = solver.solve(np.eye(4))
|
|
|
|
assert result == pytest.approx([0.0] * 7)
|
|
|
|
|
|
def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
|
|
solver = object.__new__(PlacoIkSolver)
|
|
solver._actual_joints = np.zeros(7)
|
|
solver._q_offsets = np.arange(7, 14)
|
|
solver._robot = SimpleNamespace(
|
|
state=SimpleNamespace(q=np.zeros(21)),
|
|
update_kinematics=lambda: None,
|
|
)
|
|
solver._frame_task = SimpleNamespace(T_a_b=None)
|
|
solver._solver = SimpleNamespace(solve=lambda update: None)
|
|
solver._validate_result = lambda result, previous: None
|
|
solver._update_auxiliary_task_weights = lambda: None
|
|
solver._target_errors = lambda: (2.1e-3, 0.0)
|
|
|
|
with pytest.raises(RuntimeError, match="QP did not converge after 30"):
|
|
solver.solve(np.eye(4))
|
|
|
|
|
|
def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
|
|
transform = np.eye(4)
|
|
transform[:3, 3] = [0.3, -0.1, 0.2]
|
|
|
|
actual = _validated_transform(transform)
|
|
|
|
assert actual == pytest.approx(transform)
|
|
assert actual is not transform
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"transform",
|
|
[
|
|
np.eye(3),
|
|
np.full((4, 4), np.nan),
|
|
np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
|
|
np.diag([2.0, 1.0, 1.0, 1.0]),
|
|
],
|
|
)
|
|
def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
|
|
with pytest.raises(ValueError):
|
|
_validated_transform(transform)
|
|
|
|
|
|
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
|
solver = object.__new__(PlacoIkSolver)
|
|
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
|
solver._velocity_limits = np.ones(7)
|
|
solver._dt = 0.1
|
|
solver._actual_joints = np.zeros(7)
|
|
|
|
with pytest.raises(ValueError, match="finite"):
|
|
solver._validate_result(np.full(7, np.nan))
|
|
with pytest.raises(ValueError, match="position"):
|
|
solver._validate_result(np.full(7, 2.0))
|
|
with pytest.raises(ValueError, match="velocity"):
|
|
solver._validate_result(np.full(7, 0.2))
|
|
|
|
|
|
def test_lower_margin_activation_is_clamped_and_linear() -> None:
|
|
activation = placo_ik_solver._lower_margin_activation
|
|
|
|
assert activation(0.05, 0.01, 0.04) == 0.0
|
|
assert activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
|
|
assert activation(0.005, 0.01, 0.04) == 1.0
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"arm,joint_degrees,j3_reference_deg",
|
|
[
|
|
("left", ARM_CASES[0][1], 67.96),
|
|
("right", ARM_CASES[1][1], -89.57),
|
|
],
|
|
)
|
|
def test_solver_configures_auxiliary_qp_tasks(
|
|
arm: str,
|
|
joint_degrees: list[float],
|
|
j3_reference_deg: float,
|
|
) -> None:
|
|
pytest.importorskip("placo")
|
|
solver = PlacoIkSolver(
|
|
str(DUAL_URDF_PATH),
|
|
1.0 / 90.0,
|
|
arm,
|
|
j3_reference_deg=j3_reference_deg,
|
|
j3_weight=1e-5,
|
|
j4_min_deg=10.0,
|
|
j4_warn_deg=25.0,
|
|
j4_weight=1e-4,
|
|
manipulability_sigma_stop=0.01,
|
|
manipulability_sigma_warn=0.04,
|
|
manipulability_weight=1e-4,
|
|
)
|
|
joints = np.radians(joint_degrees).tolist()
|
|
solver.update_joint_state(joints)
|
|
|
|
assert solver._j3_task.get_joint(
|
|
solver._joint_names[2]
|
|
) == pytest.approx(math.radians(j3_reference_deg))
|
|
assert np.asarray(solver._j4_constraint.A)[
|
|
solver._q_offsets[3]
|
|
] == pytest.approx(-1.0)
|
|
assert np.asarray(solver._j4_constraint.b) == pytest.approx(
|
|
[-math.radians(10.0)]
|
|
)
|
|
assert solver._j4_constraint.priority == "hard"
|
|
jacobian = solver._active_tcp_jacobian()
|
|
assert jacobian.shape == (6, 7)
|
|
assert np.isfinite(jacobian).all()
|
|
assert np.linalg.svd(jacobian, compute_uv=False)[-1] > 0.0
|
|
|
|
|
|
def test_failed_qp_restores_internal_state_to_actual_feedback() -> None:
|
|
solver, joints = _dual_placo_solver("left", ARM_CASES[0][1])
|
|
current_pose = solver.update_joint_state(joints)
|
|
unreachable = current_pose.copy()
|
|
unreachable[2, 3] += 10.0
|
|
|
|
with pytest.raises((RuntimeError, ValueError)):
|
|
solver.solve(unreachable)
|
|
|
|
assert solver._robot.state.q[solver._q_offsets] == pytest.approx(joints)
|
|
|
|
|
|
def test_solver_rejects_non_positive_manipulability_threshold() -> None:
|
|
pytest.importorskip("placo")
|
|
|
|
with pytest.raises(ValueError, match="manipulability thresholds"):
|
|
PlacoIkSolver(
|
|
str(DUAL_URDF_PATH),
|
|
1.0 / 90.0,
|
|
"left",
|
|
manipulability_sigma_stop=0.0,
|
|
manipulability_sigma_warn=0.04,
|
|
)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"q4_deg,sigma_min,expected_activation",
|
|
[
|
|
(25.0, 0.04, 0.0),
|
|
(17.5, 0.025, 0.5),
|
|
(10.0, 0.01, 1.0),
|
|
],
|
|
)
|
|
def test_auxiliary_weights_activate_only_inside_warning_margins(
|
|
q4_deg: float,
|
|
sigma_min: float,
|
|
expected_activation: float,
|
|
) -> None:
|
|
class TaskSpy:
|
|
def __init__(self) -> None:
|
|
self.calls = []
|
|
|
|
def configure(self, name, priority, weight) -> None:
|
|
self.calls.append((name, priority, weight))
|
|
|
|
solver = object.__new__(PlacoIkSolver)
|
|
solver._q_offsets = np.arange(7, 14)
|
|
solver._robot = SimpleNamespace(
|
|
state=SimpleNamespace(q=np.zeros(21))
|
|
)
|
|
solver._robot.state.q[solver._q_offsets[3]] = math.radians(q4_deg)
|
|
solver._j4_task = TaskSpy()
|
|
solver._j4_min = math.radians(10.0)
|
|
solver._j4_warn = math.radians(25.0)
|
|
solver._j4_weight = 1e-4
|
|
solver._manipulability_task = TaskSpy()
|
|
solver._manipulability_sigma_stop = 0.01
|
|
solver._manipulability_sigma_warn = 0.04
|
|
solver._manipulability_weight = 1e-4
|
|
jacobian = np.zeros((6, 7))
|
|
jacobian[:, :6] = np.diag([1.0] * 5 + [sigma_min])
|
|
solver._active_tcp_jacobian = lambda: jacobian
|
|
|
|
solver._update_auxiliary_task_weights()
|
|
|
|
expected_weight = 1e-4 * expected_activation
|
|
assert solver._j4_task.calls == [
|
|
("j4_soft_buffer", "soft", pytest.approx(expected_weight))
|
|
]
|
|
assert solver._manipulability_task.calls == [
|
|
("tcp_6d_manipulability", "soft", pytest.approx(expected_weight))
|
|
]
|