更新多点标定功能
This commit is contained in:
@@ -0,0 +1,238 @@
|
||||
"""02 multi-point acquisition without a camera or a motion controller."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
|
||||
import pytest
|
||||
|
||||
from datetime import date
|
||||
|
||||
from orisys_autocal.multipoint import Campaign, next_point_id
|
||||
|
||||
|
||||
def _campaign() -> Campaign:
|
||||
campaign = Campaign()
|
||||
campaign.add_site("A", 1, 2, 3, [5, 10], 1, 1, max_force_n=20)
|
||||
campaign.add_site("B", 4, 5, 6, [5], 1, 2, max_force_n=20)
|
||||
return campaign
|
||||
|
||||
|
||||
def _finish_open_steps(campaign: Campaign) -> None:
|
||||
site_index = campaign._site_index
|
||||
repeat_index = campaign._repeat_index
|
||||
while (
|
||||
campaign.phase == "load"
|
||||
and campaign._site_index == site_index
|
||||
and campaign._repeat_index == repeat_index
|
||||
):
|
||||
_branch, target = campaign._steps[campaign._step_index]
|
||||
campaign.confirm_reference(target)
|
||||
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||
|
||||
|
||||
def test_default_point_ids_use_the_date_and_increment():
|
||||
today = date(2026, 10, 13)
|
||||
assert next_point_id([], today) == "20261013point0"
|
||||
assert next_point_id(["20261013point0"], today) == "20261013point1"
|
||||
assert next_point_id(["20261013point0", "custom", "20261013point2"], today) == "20261013point3"
|
||||
assert next_point_id(["20261012point4"], today) == "20261013point0"
|
||||
|
||||
|
||||
def test_points_can_be_added_viewed_reordered_and_removed():
|
||||
campaign = Campaign()
|
||||
campaign.add_site("A", 0, 0, 1, [5], 10, 1, max_force_n=30)
|
||||
campaign.add_site("B", 2, 0, 1, [5, 8], 12, 3, max_force_n=30)
|
||||
assert [site.site_id for site in campaign.sites] == ["A", "B"]
|
||||
assert campaign.sites[1].repeats == 3
|
||||
campaign.move_site("B", -1)
|
||||
assert [site.site_id for site in campaign.sites] == ["B", "A"]
|
||||
campaign.remove_site("A")
|
||||
assert [site.site_id for site in campaign.sites] == ["B"]
|
||||
with pytest.raises(ValueError, match="unique"):
|
||||
campaign.add_site("B", 0, 0, 0, [5], 10, 1, max_force_n=30)
|
||||
|
||||
|
||||
def test_levels_above_the_entered_maximum_are_refused():
|
||||
campaign = Campaign()
|
||||
with pytest.raises(ValueError, match="force range"):
|
||||
campaign.add_site("A", 0, 0, 0, [40], 10, 1, max_force_n=30)
|
||||
|
||||
|
||||
def test_the_next_point_waits_until_the_indenter_is_confirmed_clear(tmp_path):
|
||||
campaign = _campaign()
|
||||
directory = campaign.start(
|
||||
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||
)
|
||||
_finish_open_steps(campaign)
|
||||
assert campaign.phase == "liftoff"
|
||||
with pytest.raises(RuntimeError, match="left the sensor"):
|
||||
campaign.confirm_reference(0)
|
||||
campaign.confirm_liftoff()
|
||||
assert campaign.phase == "load"
|
||||
assert campaign.progress()["site_id"] == "B"
|
||||
_finish_open_steps(campaign)
|
||||
assert campaign.phase == "load"
|
||||
assert campaign.progress()["repeat_index"] == 2
|
||||
_finish_open_steps(campaign)
|
||||
assert campaign.phase == "done"
|
||||
|
||||
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||
assert session["status"] == "complete"
|
||||
assert session["automatic_force_control"] is False
|
||||
assert session["platform_motion"] is False
|
||||
assert session["sync"]["association"].startswith("A frame belongs")
|
||||
assert session["coordinate_frame"] == "fixture"
|
||||
assert [site["site_id"] for site in session["sites"]] == ["A", "B"]
|
||||
assert session["sites"][0]["position"] == {"x": 1.0, "y": 2.0, "z": 3.0}
|
||||
assert all(trial["status"] == "complete" for trial in session["trials"])
|
||||
assert directory.name.startswith("finger_SN1_")
|
||||
from orisys_autocal.session_review import review_session
|
||||
with pytest.raises(ValueError, match="finger sessions"):
|
||||
review_session(directory)
|
||||
|
||||
first = directory / "sites" / "A" / "repeat-001" / "reference.csv"
|
||||
rows = list(csv.DictReader(first.open(encoding="utf-8")))
|
||||
assert [row["branch"] for row in rows] == ["baseline", "load", "load", "unload", "unload"]
|
||||
assert rows[1]["site_id"] == "A"
|
||||
assert [row["target_load_n"] for row in rows] == ["0.0", "5.0", "10.0", "5.0", "0.0"]
|
||||
assert rows[2]["F_ref_normal_N"] == "10.0"
|
||||
assert rows[2]["site_id"] == "A"
|
||||
assert rows[0]["repeat_index"] == "1"
|
||||
assert int(rows[0]["t_end_host_mono_ns"]) > int(rows[0]["t_start_host_mono_ns"])
|
||||
events = (directory / "sites" / "A" / "repeat-001" / "events.csv").read_text(encoding="utf-8")
|
||||
assert "liftoff_confirmed" not in events
|
||||
later = (directory / "sites" / "B" / "repeat-001" / "events.csv").read_text(encoding="utf-8")
|
||||
assert "trial_started" in later
|
||||
session_events = (directory / "events.csv").read_text(encoding="utf-8")
|
||||
assert "liftoff_required" in session_events
|
||||
assert "liftoff_confirmed" in session_events
|
||||
|
||||
|
||||
def test_stopping_keeps_the_points_already_recorded(tmp_path):
|
||||
campaign = _campaign()
|
||||
directory = campaign.start(
|
||||
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||
)
|
||||
_finish_open_steps(campaign)
|
||||
campaign.confirm_liftoff()
|
||||
campaign.confirm_reference(0)
|
||||
campaign.stop("operator stopped")
|
||||
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||
assert session["status"] == "incomplete"
|
||||
assert session["trials"][0]["status"] == "complete"
|
||||
assert session["trials"][1]["status"] == "incomplete"
|
||||
kept = directory / "sites" / "A" / "repeat-001" / "reference.csv"
|
||||
assert kept.is_file() and len(kept.read_text(encoding="utf-8").splitlines()) > 1
|
||||
assert campaign.phase == "aborted"
|
||||
|
||||
|
||||
def test_a_missing_frame_name_or_model_refuses_to_start(tmp_path):
|
||||
campaign = _campaign()
|
||||
with pytest.raises(ValueError, match="coordinate frame"):
|
||||
campaign.start(
|
||||
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||
coordinate_frame="", max_force_n=20, simulate=True,
|
||||
)
|
||||
with pytest.raises(ValueError, match="finger"):
|
||||
campaign.start(
|
||||
tmp_path, sensor_model="other", sensor_sn="SN1",
|
||||
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||
)
|
||||
|
||||
|
||||
_ROTATION = [0, 0, 1, 0, 1, 0, -1, 0, 0]
|
||||
|
||||
|
||||
def test_six_axis_normal_uses_the_entered_rotation_not_one_axis(tmp_path):
|
||||
campaign = Campaign()
|
||||
campaign.add_site("A", 0, 0, 1, [5], 1, 1, max_force_n=20)
|
||||
campaign.start(
|
||||
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||
)
|
||||
with pytest.raises(ValueError, match="rotation"):
|
||||
campaign.confirm_wrench(-5, 1, 9)
|
||||
with pytest.raises(ValueError, match="determinant"):
|
||||
campaign.set_force_rotation([1, 0, 0, 0, 1, 0, 0, 0, -1])
|
||||
campaign.set_force_rotation(_ROTATION)
|
||||
campaign.confirm_wrench(0, 0, 0)
|
||||
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||
campaign.confirm_wrench(-5, 1, 9)
|
||||
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||
campaign.confirm_wrench(0, 0, 0)
|
||||
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||
rows = list(csv.DictReader(
|
||||
(campaign.directory / "sites" / "A" / "repeat-001" / "reference.csv").open(encoding="utf-8")
|
||||
))
|
||||
assert rows[1]["reference_source"] == "wrench"
|
||||
assert rows[1]["branch"] == "load"
|
||||
assert float(rows[1]["F_ref_normal_N"]) == pytest.approx(5.0)
|
||||
assert float(rows[1]["fx_N"]) == pytest.approx(-5.0)
|
||||
assert float(rows[1]["fy_N"]) == pytest.approx(1.0)
|
||||
assert float(rows[1]["fz_N"]) == pytest.approx(9.0)
|
||||
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fx_N"]))
|
||||
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fy_N"]))
|
||||
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fz_N"]))
|
||||
assert campaign._document["normal_projection"].startswith("F_tool = R @ F_sensor")
|
||||
|
||||
|
||||
def test_solved_pose_is_stored_and_a_point_can_be_recorded_again(tmp_path):
|
||||
from orisys_autocal.motion.kinematics import PlatformStack
|
||||
import numpy as np
|
||||
|
||||
stack = PlatformStack()
|
||||
point = np.array([1.0, 2.0, 3.0])
|
||||
normal = np.array([0.0, 1.0, 0.0])
|
||||
pose = stack.solve(point, normal)
|
||||
assert pose.ok
|
||||
campaign = Campaign()
|
||||
campaign.add_solved_site(
|
||||
"P1", float(point[0]), float(point[1]), float(point[2]),
|
||||
(float(normal[0]), float(normal[1]), float(normal[2])),
|
||||
{
|
||||
"x": float(pose.table_xy_mm[0]),
|
||||
"y": float(pose.table_xy_mm[1]),
|
||||
"yaw": float(pose.yaw_deg),
|
||||
"pitch": float(pose.pitch_deg),
|
||||
"z": float(pose.contact_z_mm),
|
||||
"residual_deg": float(pose.residual_deg),
|
||||
"upright": bool(pose.upright),
|
||||
"in_range": bool(pose.in_range),
|
||||
"ok": bool(pose.ok),
|
||||
"normal_aligned_to": f"tool_axis:{stack.tool_axis}",
|
||||
},
|
||||
[4], 1, 1, max_force_n=10,
|
||||
)
|
||||
targets = campaign.command_targets("P1")
|
||||
assert targets["x"] == pytest.approx(pose.table_xy_mm[0])
|
||||
assert targets["y"] == pytest.approx(pose.table_xy_mm[1])
|
||||
assert targets["yaw"] == pytest.approx(pose.yaw_deg)
|
||||
assert targets["pitch"] == pytest.approx(pose.pitch_deg)
|
||||
assert targets["z"] == pytest.approx(pose.contact_z_mm)
|
||||
directory = campaign.start(
|
||||
tmp_path, sensor_model="finger", sensor_sn="SN9",
|
||||
coordinate_frame="sensor", max_force_n=10, min_force_n=0, simulate=True,
|
||||
)
|
||||
assert campaign._document["acquisition_commands_motion"] is False
|
||||
_finish_open_steps(campaign)
|
||||
assert campaign.phase == "done"
|
||||
saved = (directory / "sites" / "P1" / "repeat-001" / "reference.csv").read_text(encoding="utf-8")
|
||||
assert "pose_yaw_deg" in saved.splitlines()[0]
|
||||
row = list(csv.DictReader(saved.splitlines()))[1]
|
||||
assert float(row["pose_yaw_deg"]) == pytest.approx(pose.yaw_deg)
|
||||
assert float(row["nx"]) == pytest.approx(0.0)
|
||||
assert row["reference_source"] == "weight"
|
||||
assert row["branch"] == "load"
|
||||
campaign.reacquire("P1")
|
||||
_finish_open_steps(campaign)
|
||||
assert campaign.phase == "liftoff"
|
||||
campaign.confirm_liftoff()
|
||||
assert campaign.phase == "done"
|
||||
assert (directory / "sites" / "P1" / "repeat-002" / "reference.csv").is_file()
|
||||
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||
assert session["completion"][0]["repeats_complete"] == 2
|
||||
assert session["completion"][0]["complete"] is True
|
||||
Reference in New Issue
Block a user