cristhian
eccd9e3c24
Brings across the robot layer from monolitic-littlehand-python, which was written against this same controller. The point of it is that a command either moved the arm or raised: the SDK returns a code for a motion the controller quietly refused, wait_move calls that refusal finished, and arm.error_code is the report stream's stale copy. So errors are read live, arrival is confirmed rather than assumed, and a move whose straight-line path leaves the joint envelope is refused before it is commanded. Failures are typed, because the distinctions decide the response: an unreachable pose is not an unavailable arm, and a joint past its limit needs a motion nobody should start without warning the room. read_report is the deliberate exception to all of it: a report-cache read that costs no round trip and cannot block, which is the only read that works while a motion is in flight.
72 lines
2.2 KiB
Python
72 lines
2.2 KiB
Python
"""Pose maths and the tool-tip offset. No robot needed."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from dora_ulite6.controller import interpolate, shortest_turn
|
|
from dora_ulite6.pose import RobotPose, with_tool_offset
|
|
|
|
|
|
def test_from_sequence_accepts_a_position_alone():
|
|
pose = RobotPose.from_sequence([10, 20, 30])
|
|
|
|
assert pose.as_list() == [10, 20, 30, 180.0, 0.0, 0.0]
|
|
|
|
|
|
def test_from_sequence_round_trips_a_full_pose():
|
|
values = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
|
|
|
|
assert RobotPose.from_sequence(values).as_list() == values
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("start", "target", "expected"),
|
|
[(-178.0, 180.0, -2.0), (180.0, -178.0, 2.0), (0.0, 90.0, 90.0), (0.0, 270.0, -90.0)],
|
|
)
|
|
def test_shortest_turn_never_takes_the_long_way(start, target, expected):
|
|
assert shortest_turn(start, target) == pytest.approx(expected)
|
|
|
|
|
|
def test_interpolate_turns_the_short_way_across_the_wrap():
|
|
halfway = interpolate([0, 0, 0, -178, 0, 0], [0, 0, 0, 180, 0, 0], 0.5)
|
|
|
|
assert halfway[3] == pytest.approx(-179.0)
|
|
|
|
|
|
def test_interpolate_walks_the_straight_line_in_position():
|
|
halfway = interpolate([0, 0, 0, 180, 0, 0], [100, 200, 300, 180, 0, 0], 0.5)
|
|
|
|
assert halfway[:3] == pytest.approx([50, 100, 150])
|
|
|
|
|
|
def test_tool_down_offset_lowers_the_tip():
|
|
tip = with_tool_offset(RobotPose(300, 0, 250, roll=180, pitch=0, yaw=0), 61.0)
|
|
|
|
assert tip.z == pytest.approx(250 - 61.0)
|
|
assert (tip.x, tip.y) == pytest.approx((300, 0))
|
|
|
|
|
|
def test_offset_follows_the_tool_axis_not_the_base_axis():
|
|
# Pitched 90° from tool-down, the tool points along +x rather than -z.
|
|
tip = with_tool_offset(RobotPose(300, 0, 250, roll=0, pitch=90, yaw=0), 61.0)
|
|
|
|
assert tip.x == pytest.approx(300 + 61.0)
|
|
assert tip.z == pytest.approx(250)
|
|
|
|
|
|
def test_a_zero_offset_returns_the_pose_untouched():
|
|
pose = RobotPose(1, 2, 3, 4, 5, 6)
|
|
|
|
assert with_tool_offset(pose, 0.0) is pose
|
|
|
|
|
|
def test_the_offset_inverts_exactly():
|
|
# move_to relies on this: it takes the offset back off a tip pose to get the
|
|
# flange pose the controller wants.
|
|
pose = RobotPose(123, -45, 210, roll=170, pitch=12, yaw=-30)
|
|
|
|
there = with_tool_offset(pose, 61.0)
|
|
back = with_tool_offset(there, -61.0)
|
|
|
|
np.testing.assert_allclose(back.as_list(), pose.as_list(), atol=1e-9)
|