440 lines
16 KiB
Python
440 lines
16 KiB
Python
"""Tests for MotionAPI static/pure methods."""
|
|
import pytest
|
|
import math
|
|
import sys
|
|
import os
|
|
|
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src'))
|
|
|
|
from RSIPI.motion_api import (
|
|
MotionAPI,
|
|
_calculate_distance,
|
|
_trapezoidal_profile,
|
|
_s_curve_profile,
|
|
_cubic_blend,
|
|
_find_blend_point,
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# generate_arc
|
|
# ---------------------------------------------------------------------------
|
|
class TestGenerateArc:
|
|
"""Tests for MotionAPI.generate_arc()."""
|
|
|
|
def test_basic_arc_length(self):
|
|
"""Arc with 50 steps should return 50 waypoints."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=10.0,
|
|
start_angle=0,
|
|
end_angle=90,
|
|
steps=50,
|
|
)
|
|
assert len(arc) == 50
|
|
|
|
def test_first_point_on_circle(self):
|
|
"""First point should be at start_angle on the circle."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 100, "Y": 0, "Z": 0},
|
|
radius=50.0,
|
|
start_angle=0,
|
|
end_angle=90,
|
|
steps=10,
|
|
)
|
|
assert arc[0]["X"] == pytest.approx(150.0)
|
|
assert arc[0]["Y"] == pytest.approx(0.0)
|
|
|
|
def test_last_point_at_end_angle(self):
|
|
"""Last point should be at end_angle on the circle."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=10.0,
|
|
start_angle=0,
|
|
end_angle=90,
|
|
steps=10,
|
|
)
|
|
assert arc[-1]["X"] == pytest.approx(10.0 * math.cos(math.radians(90)))
|
|
assert arc[-1]["Y"] == pytest.approx(10.0 * math.sin(math.radians(90)))
|
|
|
|
def test_xz_plane(self):
|
|
"""Arc in XZ plane should vary X and Z, keep Y constant."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 5, "Z": 0},
|
|
radius=10.0,
|
|
start_angle=0,
|
|
end_angle=90,
|
|
steps=5,
|
|
plane="XZ",
|
|
)
|
|
for pt in arc:
|
|
assert pt["Y"] == 5
|
|
|
|
def test_yz_plane(self):
|
|
"""Arc in YZ plane should vary Y and Z, keep X constant."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 7, "Y": 0, "Z": 0},
|
|
radius=10.0,
|
|
start_angle=0,
|
|
end_angle=180,
|
|
steps=5,
|
|
plane="YZ",
|
|
)
|
|
for pt in arc:
|
|
assert pt["X"] == 7
|
|
|
|
def test_preserves_orientation(self):
|
|
"""Orientation keys A, B, C should carry through from center."""
|
|
arc = MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0, "A": 10, "B": 20, "C": 30},
|
|
radius=5.0,
|
|
start_angle=0,
|
|
end_angle=45,
|
|
steps=3,
|
|
)
|
|
for pt in arc:
|
|
assert pt["A"] == 10
|
|
assert pt["B"] == 20
|
|
assert pt["C"] == 30
|
|
|
|
def test_invalid_plane_raises(self):
|
|
with pytest.raises(ValueError, match="Unknown plane"):
|
|
MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=5.0, start_angle=0, end_angle=90, steps=5, plane="AB"
|
|
)
|
|
|
|
def test_zero_radius_raises(self):
|
|
with pytest.raises(ValueError, match="positive"):
|
|
MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=0.0, start_angle=0, end_angle=90, steps=5
|
|
)
|
|
|
|
def test_one_step_raises(self):
|
|
with pytest.raises(ValueError, match="at least 2"):
|
|
MotionAPI.generate_arc(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=5.0, start_angle=0, end_angle=90, steps=1
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# generate_circle
|
|
# ---------------------------------------------------------------------------
|
|
class TestGenerateCircle:
|
|
"""Tests for MotionAPI.generate_circle()."""
|
|
|
|
def test_returns_360_degree_arc(self):
|
|
"""Circle should span full 360 degrees."""
|
|
circle = MotionAPI.generate_circle(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
radius=10.0,
|
|
steps=100,
|
|
)
|
|
assert len(circle) == 100
|
|
# First and last points should be nearly identical (full revolution)
|
|
assert circle[0]["X"] == pytest.approx(circle[-1]["X"], abs=0.5)
|
|
assert circle[0]["Y"] == pytest.approx(circle[-1]["Y"], abs=0.5)
|
|
|
|
def test_circle_points_on_radius(self):
|
|
"""All points should lie on the specified radius from center."""
|
|
cx, cy = 50.0, 30.0
|
|
radius = 20.0
|
|
circle = MotionAPI.generate_circle(
|
|
center={"X": cx, "Y": cy, "Z": 0},
|
|
radius=radius,
|
|
steps=36,
|
|
)
|
|
for pt in circle:
|
|
dist = math.sqrt((pt["X"] - cx) ** 2 + (pt["Y"] - cy) ** 2)
|
|
assert dist == pytest.approx(radius, abs=1e-10)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# generate_spiral
|
|
# ---------------------------------------------------------------------------
|
|
class TestGenerateSpiral:
|
|
"""Tests for MotionAPI.generate_spiral()."""
|
|
|
|
def test_expanding_spiral_radius_increases(self):
|
|
"""Radius should increase from start_radius to end_radius."""
|
|
spiral = MotionAPI.generate_spiral(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
start_radius=5.0,
|
|
end_radius=50.0,
|
|
pitch=0.0,
|
|
revolutions=2,
|
|
steps=50,
|
|
)
|
|
first_r = math.sqrt(spiral[0]["X"] ** 2 + spiral[0]["Y"] ** 2)
|
|
last_r = math.sqrt(spiral[-1]["X"] ** 2 + spiral[-1]["Y"] ** 2)
|
|
assert last_r > first_r
|
|
|
|
def test_contracting_spiral_radius_decreases(self):
|
|
"""Radius should decrease when end_radius < start_radius."""
|
|
spiral = MotionAPI.generate_spiral(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
start_radius=50.0,
|
|
end_radius=5.0,
|
|
pitch=0.0,
|
|
revolutions=2,
|
|
steps=50,
|
|
)
|
|
first_r = math.sqrt(spiral[0]["X"] ** 2 + spiral[0]["Y"] ** 2)
|
|
last_r = math.sqrt(spiral[-1]["X"] ** 2 + spiral[-1]["Y"] ** 2)
|
|
assert last_r < first_r
|
|
|
|
def test_pitch_adds_height(self):
|
|
"""Positive pitch should increase Z across the spiral."""
|
|
spiral = MotionAPI.generate_spiral(
|
|
center={"X": 0, "Y": 0, "Z": 100},
|
|
start_radius=10.0,
|
|
end_radius=10.0,
|
|
pitch=20.0,
|
|
revolutions=1,
|
|
steps=20,
|
|
)
|
|
assert spiral[-1]["Z"] > spiral[0]["Z"]
|
|
|
|
def test_step_count(self):
|
|
"""Should return exactly the requested number of steps."""
|
|
spiral = MotionAPI.generate_spiral(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
start_radius=10.0,
|
|
end_radius=20.0,
|
|
pitch=5.0,
|
|
steps=33,
|
|
)
|
|
assert len(spiral) == 33
|
|
|
|
def test_too_few_steps_raises(self):
|
|
with pytest.raises(ValueError, match="at least 2"):
|
|
MotionAPI.generate_spiral(
|
|
center={"X": 0, "Y": 0, "Z": 0},
|
|
start_radius=5, end_radius=10, pitch=1, steps=1
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# blend_trajectories
|
|
# ---------------------------------------------------------------------------
|
|
class TestBlendTrajectories:
|
|
"""Tests for MotionAPI.blend_trajectories()."""
|
|
|
|
@staticmethod
|
|
def _line(start_x, end_x, n=20):
|
|
"""Helper: straight line along X."""
|
|
return [{"X": start_x + i * (end_x - start_x) / (n - 1), "Y": 0, "Z": 0} for i in range(n)]
|
|
|
|
def test_blended_length(self):
|
|
"""Blended trajectory should have points from both segments plus blend zone."""
|
|
t1 = self._line(0, 100, 20)
|
|
t2 = self._line(100, 200, 20)
|
|
blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=10.0, blend_steps=10)
|
|
# Should have some points; exact count depends on blend_radius vs traj length
|
|
assert len(blended) > 0
|
|
|
|
def test_blended_starts_at_traj1_start(self):
|
|
"""First point of blended should match first point of traj1."""
|
|
t1 = self._line(0, 100, 20)
|
|
t2 = self._line(100, 200, 20)
|
|
blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=5.0, blend_steps=10)
|
|
assert blended[0]["X"] == pytest.approx(0.0)
|
|
|
|
def test_blended_ends_at_traj2_end(self):
|
|
"""Last point of blended should match last point of traj2."""
|
|
t1 = self._line(0, 100, 20)
|
|
t2 = self._line(100, 200, 20)
|
|
blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=5.0, blend_steps=10)
|
|
assert blended[-1]["X"] == pytest.approx(200.0)
|
|
|
|
def test_empty_traj_raises(self):
|
|
with pytest.raises(ValueError, match="non-empty"):
|
|
MotionAPI.blend_trajectories([], [{"X": 0}], blend_radius=1.0)
|
|
|
|
def test_zero_blend_radius_raises(self):
|
|
with pytest.raises(ValueError, match="positive"):
|
|
MotionAPI.blend_trajectories(
|
|
[{"X": 0}], [{"X": 1}], blend_radius=0.0
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# transform_coordinates
|
|
# ---------------------------------------------------------------------------
|
|
class TestTransformCoordinates:
|
|
"""Tests for MotionAPI.transform_coordinates()."""
|
|
|
|
def test_no_offset_returns_copy(self):
|
|
"""No frame_offset should return a copy of the input pose."""
|
|
pose = {"X": 10, "Y": 20, "Z": 30}
|
|
result = MotionAPI.transform_coordinates(pose)
|
|
assert result == pose
|
|
# Should be a copy, not the same object
|
|
assert result is not pose
|
|
|
|
def test_translational_offset(self):
|
|
"""Frame offset should add to position values."""
|
|
pose = {"X": 100, "Y": 0, "Z": 500}
|
|
offset = {"X": 500, "Y": 200, "Z": 0}
|
|
result = MotionAPI.transform_coordinates(pose, frame_offset=offset)
|
|
assert result["X"] == 600
|
|
assert result["Y"] == 200
|
|
assert result["Z"] == 500
|
|
|
|
def test_rotational_offset(self):
|
|
"""Frame offset should add to orientation values."""
|
|
pose = {"X": 0, "Y": 0, "Z": 0, "A": 10, "B": 20, "C": 30}
|
|
offset = {"A": 5, "B": -10, "C": 15}
|
|
result = MotionAPI.transform_coordinates(pose, frame_offset=offset)
|
|
assert result["A"] == 15
|
|
assert result["B"] == 10
|
|
assert result["C"] == 45
|
|
|
|
def test_partial_offset(self):
|
|
"""Offset with fewer keys should only affect matching axes."""
|
|
pose = {"X": 100, "Y": 200, "Z": 300}
|
|
offset = {"X": 10}
|
|
result = MotionAPI.transform_coordinates(pose, frame_offset=offset)
|
|
assert result["X"] == 110
|
|
assert result["Y"] == 200 # unchanged
|
|
assert result["Z"] == 300 # unchanged
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# generate_velocity_profile
|
|
# ---------------------------------------------------------------------------
|
|
class TestGenerateVelocityProfile:
|
|
"""Tests for MotionAPI.generate_velocity_profile()."""
|
|
|
|
@staticmethod
|
|
def _simple_traj(n=50):
|
|
"""Evenly spaced points along X."""
|
|
return [{"X": i * 1.0, "Y": 0, "Z": 0} for i in range(n)]
|
|
|
|
def test_trapezoidal_returns_correct_length(self):
|
|
traj = self._simple_traj(50)
|
|
result = MotionAPI.generate_velocity_profile(traj, profile='trapezoidal')
|
|
assert len(result) == 50
|
|
|
|
def test_scurve_returns_correct_length(self):
|
|
traj = self._simple_traj(50)
|
|
result = MotionAPI.generate_velocity_profile(traj, profile='s-curve')
|
|
assert len(result) == 50
|
|
|
|
def test_trapezoidal_starts_and_ends_near_zero(self):
|
|
"""Velocity should be near zero at start and end."""
|
|
traj = self._simple_traj(100)
|
|
result = MotionAPI.generate_velocity_profile(
|
|
traj, max_velocity=10.0, max_acceleration=5.0, profile='trapezoidal'
|
|
)
|
|
_, v_first = result[0]
|
|
_, v_last = result[-1]
|
|
assert v_first == pytest.approx(0.0, abs=0.5)
|
|
assert v_last == pytest.approx(0.0, abs=0.5)
|
|
|
|
def test_scurve_starts_and_ends_near_zero(self):
|
|
traj = self._simple_traj(100)
|
|
result = MotionAPI.generate_velocity_profile(
|
|
traj, max_velocity=10.0, max_acceleration=5.0, profile='s-curve'
|
|
)
|
|
_, v_first = result[0]
|
|
_, v_last = result[-1]
|
|
assert v_first == pytest.approx(0.0, abs=0.5)
|
|
assert v_last == pytest.approx(0.0, abs=0.5)
|
|
|
|
def test_empty_trajectory(self):
|
|
assert MotionAPI.generate_velocity_profile([]) == []
|
|
|
|
def test_single_point(self):
|
|
result = MotionAPI.generate_velocity_profile([{"X": 0}])
|
|
assert len(result) == 1
|
|
assert result[0][1] == 0.0
|
|
|
|
def test_unknown_profile_raises(self):
|
|
traj = self._simple_traj(10)
|
|
with pytest.raises(ValueError, match="Unknown profile"):
|
|
MotionAPI.generate_velocity_profile(traj, profile='banana')
|
|
|
|
def test_trapezoidal_velocity_does_not_exceed_max(self):
|
|
"""No velocity should exceed max_velocity."""
|
|
traj = self._simple_traj(100)
|
|
max_v = 5.0
|
|
result = MotionAPI.generate_velocity_profile(
|
|
traj, max_velocity=max_v, max_acceleration=2.0, profile='trapezoidal'
|
|
)
|
|
for _, v in result:
|
|
assert v <= max_v + 1e-9
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# _cubic_blend edge cases
|
|
# ---------------------------------------------------------------------------
|
|
class TestCubicBlend:
|
|
"""Tests for _cubic_blend helper."""
|
|
|
|
def test_steps_one_no_crash(self):
|
|
"""steps=1 should not cause division by zero."""
|
|
result = _cubic_blend({"X": 0, "Y": 0}, {"X": 10, "Y": 10}, steps=1)
|
|
assert len(result) == 1
|
|
|
|
def test_steps_two(self):
|
|
"""steps=2 should interpolate from p1 to p2."""
|
|
result = _cubic_blend({"X": 0}, {"X": 100}, steps=2)
|
|
assert len(result) == 2
|
|
assert result[0]["X"] == pytest.approx(0.0)
|
|
assert result[-1]["X"] == pytest.approx(100.0)
|
|
|
|
def test_midpoint_is_blended(self):
|
|
"""Midpoint (t=0.5) of cubic Hermite with zero velocities should be average."""
|
|
result = _cubic_blend({"X": 0}, {"X": 100}, steps=3)
|
|
# At t=0.5: h1=0.5, h2=0.5 -> blended = 50
|
|
assert result[1]["X"] == pytest.approx(50.0)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# _trapezoidal_profile edge cases
|
|
# ---------------------------------------------------------------------------
|
|
class TestTrapezoidalProfileEdgeCases:
|
|
"""Edge-case tests for the _trapezoidal_profile helper."""
|
|
|
|
def test_single_distance(self):
|
|
"""Profile with one distance segment should not crash."""
|
|
velocities = _trapezoidal_profile([10.0], 10.0, 5.0, 2.0)
|
|
assert len(velocities) == 2 # n = len(distances) + 1
|
|
|
|
def test_zero_total_distance(self):
|
|
"""Zero total distance should produce zero velocities without error."""
|
|
velocities = _trapezoidal_profile([0.0], 0.0, 5.0, 2.0)
|
|
assert all(v == 0.0 for v in velocities)
|
|
|
|
def test_triangular_profile_triggered(self):
|
|
"""Short distance forces triangular profile (no constant phase)."""
|
|
# accel_distance = v^2 / (2*a) = 100/4 = 25
|
|
# total = 10 < 2*25 => triangular
|
|
velocities = _trapezoidal_profile([5.0, 5.0], 10.0, 10.0, 2.0)
|
|
assert len(velocities) == 3
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# _calculate_distance
|
|
# ---------------------------------------------------------------------------
|
|
class TestCalculateDistance:
|
|
"""Tests for _calculate_distance helper."""
|
|
|
|
def test_zero_distance(self):
|
|
assert _calculate_distance({"X": 0, "Y": 0}, {"X": 0, "Y": 0}) == 0.0
|
|
|
|
def test_unit_distance(self):
|
|
assert _calculate_distance({"X": 0}, {"X": 1}) == pytest.approx(1.0)
|
|
|
|
def test_3d_distance(self):
|
|
d = _calculate_distance({"X": 0, "Y": 0, "Z": 0}, {"X": 3, "Y": 4, "Z": 0})
|
|
assert d == pytest.approx(5.0)
|
|
|
|
def test_ignores_non_motion_keys(self):
|
|
"""Keys not in the motion set should be ignored."""
|
|
d = _calculate_distance({"X": 0, "FOO": 0}, {"X": 3, "FOO": 1000})
|
|
assert d == pytest.approx(3.0)
|