robot-models-lib 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (96) hide show
  1. robot_models/__init__.py +18 -0
  2. robot_models/actuators/__init__.py +1 -0
  3. robot_models/actuators/rungs/README.md +24 -0
  4. robot_models/actuators/rungs/coulomb_viscous/README.md +61 -0
  5. robot_models/actuators/rungs/coulomb_viscous/model.js +14 -0
  6. robot_models/actuators/rungs/coulomb_viscous/model.json +102 -0
  7. robot_models/actuators/rungs/coulomb_viscous/model.py +24 -0
  8. robot_models/actuators/rungs/datasheet/README.md +65 -0
  9. robot_models/actuators/rungs/datasheet/model.js +14 -0
  10. robot_models/actuators/rungs/datasheet/model.json +138 -0
  11. robot_models/actuators/rungs/datasheet/model.py +24 -0
  12. robot_models/actuators/rungs/dcmotor/README.md +63 -0
  13. robot_models/actuators/rungs/dcmotor/model.json +163 -0
  14. robot_models/actuators/rungs/ideal/README.md +60 -0
  15. robot_models/actuators/rungs/ideal/model.js +6 -0
  16. robot_models/actuators/rungs/ideal/model.json +90 -0
  17. robot_models/actuators/rungs/ideal/model.py +13 -0
  18. robot_models/actuators/rungs/load/README.md +65 -0
  19. robot_models/actuators/rungs/load/model.js +15 -0
  20. robot_models/actuators/rungs/load/model.json +117 -0
  21. robot_models/actuators/rungs/load/model.py +25 -0
  22. robot_models/actuators/rungs/lugre/README.md +72 -0
  23. robot_models/actuators/rungs/lugre/model.js +24 -0
  24. robot_models/actuators/rungs/lugre/model.json +164 -0
  25. robot_models/actuators/rungs/lugre/model.py +30 -0
  26. robot_models/actuators/rungs/ripple/README.md +64 -0
  27. robot_models/actuators/rungs/ripple/model.js +17 -0
  28. robot_models/actuators/rungs/ripple/model.json +119 -0
  29. robot_models/actuators/rungs/ripple/model.py +27 -0
  30. robot_models/actuators/rungs/stribeck/README.md +65 -0
  31. robot_models/actuators/rungs/stribeck/model.js +17 -0
  32. robot_models/actuators/rungs/stribeck/model.json +126 -0
  33. robot_models/actuators/rungs/stribeck/model.py +28 -0
  34. robot_models/actuators/servo/README.md +278 -0
  35. robot_models/actuators/servo/__init__.py +7 -0
  36. robot_models/actuators/servo/bldc/README.md +186 -0
  37. robot_models/actuators/servo/bldc/__init__.py +5 -0
  38. robot_models/actuators/servo/bldc/bldc_motor.py +93 -0
  39. robot_models/actuators/servo/bldc/ladder.py +268 -0
  40. robot_models/actuators/servo/foc_controller/README.md +127 -0
  41. robot_models/actuators/servo/foc_controller/__init__.py +3 -0
  42. robot_models/actuators/servo/foc_controller/ladder.py +207 -0
  43. robot_models/actuators/servo/gearbox/README.md +148 -0
  44. robot_models/actuators/servo/gearbox/__init__.py +5 -0
  45. robot_models/actuators/servo/gearbox/gearbox.py +36 -0
  46. robot_models/actuators/servo/gearbox/ladder.py +214 -0
  47. robot_models/actuators/servo/integrated_servo.py +33 -0
  48. robot_models/actuators/servo/ladder.py +593 -0
  49. robot_models/arms/__init__.py +1 -0
  50. robot_models/arms/rigid_body_chain/README.md +151 -0
  51. robot_models/arms/rigid_body_chain/__init__.py +3 -0
  52. robot_models/arms/rigid_body_chain/ladder.py +228 -0
  53. robot_models/conveyors/__init__.py +1 -0
  54. robot_models/conveyors/conveyor/README.md +140 -0
  55. robot_models/conveyors/conveyor/__init__.py +1 -0
  56. robot_models/conveyors/conveyor/ladder.py +203 -0
  57. robot_models/datasheet/__init__.py +56 -0
  58. robot_models/datasheet/datasheet.py +355 -0
  59. robot_models/datasheet/literal.py +144 -0
  60. robot_models/datasheet/source.py +328 -0
  61. robot_models/datasheet/stated.py +135 -0
  62. robot_models/datasheet/units.py +57 -0
  63. robot_models/end_effectors/__init__.py +1 -0
  64. robot_models/end_effectors/gripper/README.md +112 -0
  65. robot_models/end_effectors/gripper/__init__.py +3 -0
  66. robot_models/end_effectors/gripper/ladder.py +150 -0
  67. robot_models/homing/__init__.py +38 -0
  68. robot_models/homing/cad_bundle.py +206 -0
  69. robot_models/homing/contact.py +201 -0
  70. robot_models/homing/fit.py +126 -0
  71. robot_models/homing/metrics.py +237 -0
  72. robot_models/homing/protocols.py +72 -0
  73. robot_models/homing/types.py +256 -0
  74. robot_models/ladder.py +645 -0
  75. robot_models/markers/__init__.py +1 -0
  76. robot_models/markers/aruco_marker/README.md +92 -0
  77. robot_models/markers/aruco_marker/__init__.py +2 -0
  78. robot_models/markers/aruco_marker/ladder.py +142 -0
  79. robot_models/references.py +429 -0
  80. robot_models/sensors/__init__.py +1 -0
  81. robot_models/sensors/camera/README.md +118 -0
  82. robot_models/sensors/camera/__init__.py +3 -0
  83. robot_models/sensors/camera/ladder.py +108 -0
  84. robot_models/sensors/encoder/README.md +103 -0
  85. robot_models/sensors/encoder/__init__.py +5 -0
  86. robot_models/sensors/encoder/ladder.py +133 -0
  87. robot_models/sensors/hyperspectral_camera/README.md +107 -0
  88. robot_models/sensors/hyperspectral_camera/__init__.py +1 -0
  89. robot_models/sensors/hyperspectral_camera/ladder.py +151 -0
  90. robot_models/sensors/rgbd/README.md +113 -0
  91. robot_models/sensors/rgbd/__init__.py +5 -0
  92. robot_models/sensors/rgbd/ladder.py +113 -0
  93. robot_models_lib-0.2.0.dist-info/METADATA +143 -0
  94. robot_models_lib-0.2.0.dist-info/RECORD +96 -0
  95. robot_models_lib-0.2.0.dist-info/WHEEL +4 -0
  96. robot_models_lib-0.2.0.dist-info/licenses/LICENSE +21 -0
@@ -0,0 +1,201 @@
1
+ """Exact first contact of two mesh sets turned about one joint axis (the ``contact`` extra: coal).
2
+
3
+ The kinematic homing rung: rigid, no softness. It is the cad2robot exporter's algorithm 2
4
+ (``cad2robot/hardstops.py``, python-fcl) ported to coal, the hpp-fcl successor:
5
+
6
+ * sweep conservative advancement capped at ``step_rad``: a moving mesh's surface moves at
7
+ most ``r_max * dq``, so a pair ``d`` apart cannot touch within ``(d - contact) / r_max``.
8
+ * refine bisection pins first contact to ``tolerance_rad``.
9
+ * result the last angle verified clear (the exporter's ``q_rad`` semantics); ``None`` when a
10
+ full turn never touches.
11
+
12
+ ``contact_m`` is the touching criterion: ``CONTACT_M`` (0.05 mm, the exporter's proximity
13
+ ``contact_mm``) or ``INTERFERENCE_M`` (the meshes overlap), which the exporter's loop-joint
14
+ sweep records as ``distance_mm: null``.
15
+ """
16
+
17
+ from __future__ import annotations
18
+
19
+ import math
20
+ from collections.abc import Sequence
21
+ from dataclasses import dataclass
22
+
23
+ import numpy as np
24
+
25
+ CONTACT_M = 5e-5
26
+ # WHY 1 nm and not 0: coal reports 0 (or a negative depth) once two BVH meshes overlap.
27
+ INTERFERENCE_M = 1e-9
28
+ STEP_RAD = math.radians(1.0)
29
+ TOLERANCE_RAD = math.radians(0.05) / 8
30
+
31
+
32
+ @dataclass(frozen=True, eq=False)
33
+ class Mesh:
34
+ """A triangle mesh placed in the robot's world frame at the export pose (metres)."""
35
+
36
+ vertices: np.ndarray
37
+ faces: np.ndarray
38
+ name: str = ""
39
+
40
+ def rotated(self, axis_point: Sequence[float], axis: Sequence[float], angle_rad: float) -> "Mesh":
41
+ """The same mesh turned by ``angle_rad`` about the axis (used to move a stop on purpose)."""
42
+ point = np.asarray(axis_point, float)
43
+ rotation = rotation_matrix(axis, angle_rad)
44
+ return Mesh((self.vertices - point) @ rotation.T + point, self.faces, self.name)
45
+
46
+
47
+ def rotation_matrix(axis: Sequence[float], angle_rad: float) -> np.ndarray:
48
+ """Rodrigues: rotation by ``angle_rad`` about the unit ``axis``."""
49
+ x, y, z = np.asarray(axis, float) / np.linalg.norm(axis)
50
+ c, s = math.cos(angle_rad), math.sin(angle_rad)
51
+ t = 1.0 - c
52
+ return np.array(
53
+ [
54
+ [c + x * x * t, x * y * t - z * s, x * z * t + y * s],
55
+ [y * x * t + z * s, c + y * y * t, y * z * t - x * s],
56
+ [z * x * t - y * s, z * y * t + x * s, c + z * z * t],
57
+ ]
58
+ )
59
+
60
+
61
+ @dataclass(frozen=True)
62
+ class FirstContact:
63
+ """``q_rad``: signed joint angle last verified clear; first contact lies within tolerance past it."""
64
+
65
+ q_rad: float
66
+ distance_m: float
67
+ point_m: tuple[float, float, float]
68
+ moving_index: int
69
+ fixed_index: int
70
+ queries: int
71
+
72
+ @property
73
+ def q_deg(self) -> float:
74
+ return math.degrees(self.q_rad)
75
+
76
+
77
+ def first_contact(
78
+ moving: Sequence[Mesh],
79
+ fixed: Sequence[Mesh],
80
+ axis_point: Sequence[float],
81
+ axis: Sequence[float],
82
+ side: int,
83
+ *,
84
+ contact_m: float = CONTACT_M,
85
+ step_rad: float = STEP_RAD,
86
+ tolerance_rad: float = TOLERANCE_RAD,
87
+ max_rad: float = 2.0 * math.pi,
88
+ ) -> FirstContact | None:
89
+ """Turn ``moving`` by ``side * q`` about the axis from q = 0 until it first touches ``fixed``.
90
+
91
+ Touching means ``distance < contact_m``. A pair touching at q = 0 is walked away in
92
+ ``tolerance_rad`` steps for up to ``RESTING_PROBE_RAD``: if it frees, the sweep goes on from
93
+ there; if not, the joint rests on this stop and the result is 0.
94
+ """
95
+ if side not in (-1, 1):
96
+ raise ValueError(f"side must be -1 or +1, not {side!r}")
97
+ if not moving or not fixed:
98
+ return None
99
+ sweep = _Sweep(moving, fixed, axis_point, axis, side, contact_m)
100
+ floor = tolerance_rad
101
+ clear = np.zeros((len(moving), len(fixed))) # angle below which each pair cannot touch
102
+ q, last = 0.0, 0.0
103
+ if sweep.touching(0.0):
104
+ while q < RESTING_PROBE_RAD and sweep.touching(q):
105
+ q += floor
106
+ if sweep.touching(q):
107
+ return sweep.result(0.0)
108
+ last = q - floor
109
+ clear[:] = q
110
+ while True:
111
+ due = np.argwhere(clear <= q + 1e-15)
112
+ for i, j in due:
113
+ distance = sweep.distance(int(i), int(j), q)
114
+ if distance < contact_m:
115
+ return sweep.result(_refine(sweep, last, q, tolerance_rad))
116
+ # WHY r_max: the moving surface travels at most r_max * dq, so this pair stays clear
117
+ # until q + (distance - contact) / r_max (the exporter's conservative advancement).
118
+ clear[i, j] = q + (distance - contact_m) / sweep.r_max[i]
119
+ if q >= max_rad:
120
+ return None
121
+ last = q
122
+ q = min(max(min(float(clear.min()), q + step_rad), q + floor), max_rad)
123
+
124
+
125
+ RESTING_PROBE_RAD = math.radians(5.0)
126
+
127
+
128
+ class _Sweep:
129
+ def __init__(self, moving, fixed, axis_point, axis, side, contact_m):
130
+ import coal # noqa: PLC0415 -- the contact extra
131
+
132
+ self.coal = coal
133
+ self.point = np.asarray(axis_point, float)
134
+ self.axis = np.asarray(axis, float) / np.linalg.norm(axis)
135
+ self.side, self.contact_m = side, contact_m
136
+ self.movers = [self._bvh(mesh) for mesh in moving]
137
+ self.stays = [self._bvh(mesh) for mesh in fixed]
138
+ self.r_max = np.array(
139
+ [max(float(np.linalg.norm(np.cross(m.vertices - self.point, self.axis), axis=1).max()), 1e-9) for m in moving]
140
+ )
141
+ self.identity = coal.Transform3s()
142
+ self.queries = 0
143
+ self._pose_q: float | None = None
144
+ self._pose = None
145
+
146
+ def _bvh(self, mesh: Mesh):
147
+ model = self.coal.BVHModelOBBRSS()
148
+ model.beginModel(len(mesh.faces), len(mesh.vertices))
149
+ model.addVertices(np.ascontiguousarray(mesh.vertices, float))
150
+ model.addTriangles(np.ascontiguousarray(mesh.faces, np.int64))
151
+ model.endModel()
152
+ return model
153
+
154
+ def pose(self, q: float):
155
+ if self._pose_q != q:
156
+ rotation = rotation_matrix(self.axis, self.side * q)
157
+ self._pose = self.coal.Transform3s(rotation, self.point - rotation @ self.point)
158
+ self._pose_q = q
159
+ return self._pose
160
+
161
+ def query(self, i: int, j: int, q: float):
162
+ """coal always fills the nearest points (enable_nearest_points is deprecated in coal 3)."""
163
+ self.queries += 1
164
+ request = self.coal.DistanceRequest()
165
+ result = self.coal.DistanceResult()
166
+ distance = float(self.coal.distance(self.movers[i], self.pose(q), self.stays[j], self.identity, request, result))
167
+ return distance, result
168
+
169
+ def distance(self, i: int, j: int, q: float) -> float:
170
+ return self.query(i, j, q)[0]
171
+
172
+ def closest(self, q: float) -> tuple[float, int, int]:
173
+ best = (math.inf, -1, -1)
174
+ for i in range(len(self.movers)):
175
+ for j in range(len(self.stays)):
176
+ distance = self.distance(i, j, q)
177
+ if distance < best[0]:
178
+ best = (distance, i, j)
179
+ return best
180
+
181
+ def touching(self, q: float) -> bool:
182
+ return self.closest(q)[0] < self.contact_m
183
+
184
+ def result(self, q_clear: float) -> FirstContact:
185
+ _, i, j = self.closest(q_clear)
186
+ distance, result = self.query(i, j, q_clear)
187
+ point = 0.5 * (np.asarray(result.getNearestPoint1()) + np.asarray(result.getNearestPoint2()))
188
+ return FirstContact(
189
+ self.side * q_clear + 0.0, max(distance, 0.0), tuple(float(x) for x in point), i, j, self.queries
190
+ )
191
+
192
+
193
+ def _refine(sweep: _Sweep, low: float, high: float, tolerance_rad: float) -> float:
194
+ """Bisect between the last clear angle and the first touching one; return the last clear."""
195
+ while high - low > tolerance_rad:
196
+ middle = 0.5 * (low + high)
197
+ if sweep.touching(middle):
198
+ high = middle
199
+ else:
200
+ low = middle
201
+ return low
@@ -0,0 +1,126 @@
1
+ """Pure fit: settled stalls in, a zero per joint and a backlash table out (numpy + stdlib only).
2
+
3
+ The forward model it inverts (docs/peps/0001, sign convention in ``types``)::
4
+
5
+ motor_raw = q_stop(side) + side * (gap / 2 + compliance * torque) - offset
6
+
7
+ ``identify`` sets how far up the B0 / B1 / B2 ladder the stalls are asked to go:
8
+
9
+ * compliance (B2) needs two torque levels on one side: the slope of motor_raw against torque.
10
+ * gap (B1, B2) needs both sides of a joint whose stops both exist: what the motor travels
11
+ between the two stalls minus the CAD span. WHY flagged sim-only in the PEP: on a real robot
12
+ a CAD span error aliases straight into this gap.
13
+ * A part that the stalls cannot show is reported unobservable (a prior or 0), never as a
14
+ measured 0.
15
+
16
+ ``table`` (the boot path) skips identification and applies the stored backlash instead.
17
+ """
18
+
19
+ from __future__ import annotations
20
+
21
+ from collections.abc import Sequence
22
+
23
+ from robot_models.homing.types import (
24
+ BACKLASH_KINDS,
25
+ LOWER,
26
+ UPPER,
27
+ BacklashModel,
28
+ BacklashTable,
29
+ HomingResult,
30
+ JointBacklash,
31
+ JointZero,
32
+ StallEvidence,
33
+ StopMap,
34
+ )
35
+
36
+
37
+ def fit(
38
+ evidence: Sequence[StallEvidence],
39
+ stops: StopMap,
40
+ *,
41
+ identify: str = "B2",
42
+ table: BacklashTable | None = None,
43
+ rung: str | None = None,
44
+ ) -> HomingResult:
45
+ """Zero every joint the stalls touch; identify its backlash unless ``table`` gives it."""
46
+ if identify not in BACKLASH_KINDS:
47
+ raise ValueError(f"identify must be one of {BACKLASH_KINDS}, not {identify!r}")
48
+ evidence = tuple(evidence)
49
+ if not evidence:
50
+ raise ValueError("fit needs at least one stall")
51
+ by_joint: dict[str, list[StallEvidence]] = {}
52
+ for stall in evidence:
53
+ if stops.q_rad(stall.joint, stall.side) is None:
54
+ raise ValueError(
55
+ f"{stall.joint} stalled on its {'upper' if stall.side > 0 else 'lower'} side, "
56
+ "but the stop map has no stop there"
57
+ )
58
+ by_joint.setdefault(stall.joint, []).append(stall)
59
+
60
+ zeros, entries = [], []
61
+ for joint, stalls in by_joint.items():
62
+ stored = table.get(joint) if table is not None else None
63
+ entry = stored if stored is not None else _identify(joint, stalls, stops, identify)
64
+ offsets = [
65
+ stops.q_rad(joint, s.side) + entry.model.motor_past_link_rad(s.side, s.torque_nm) - s.motor_raw_rad
66
+ for s in stalls
67
+ ]
68
+ zeros.append(
69
+ JointZero(
70
+ joint,
71
+ sum(offsets) / len(offsets),
72
+ max(offsets) - min(offsets),
73
+ len(offsets),
74
+ tuple(sorted({s.side for s in stalls})),
75
+ )
76
+ )
77
+ entries.append(entry)
78
+ return HomingResult(tuple(zeros), BacklashTable(tuple(entries)), stops, evidence, rung or evidence[0].rung)
79
+
80
+
81
+ def _identify(joint: str, stalls: list[StallEvidence], stops: StopMap, kind: str) -> JointBacklash:
82
+ sides = {side: [s for s in stalls if s.side == side] for side in (LOWER, UPPER)}
83
+ notes = []
84
+
85
+ # compliance: side * raw = side * level + compliance * torque on each side; pooled slope
86
+ compliance, compliance_observable = 0.0, False
87
+ if kind == "B2":
88
+ numerator = denominator = 0.0
89
+ for side, items in sides.items():
90
+ torques = [s.torque_nm for s in items]
91
+ if len(set(torques)) < 2:
92
+ continue
93
+ mean_torque = sum(torques) / len(torques)
94
+ mean_y = sum(side * s.motor_raw_rad for s in items) / len(items)
95
+ numerator += sum((s.torque_nm - mean_torque) * (side * s.motor_raw_rad - mean_y) for s in items)
96
+ denominator += sum((s.torque_nm - mean_torque) ** 2 for s in items)
97
+ if denominator > 0:
98
+ compliance_observable = True
99
+ compliance = numerator / denominator
100
+ notes.append("compliance: two-level torque slope")
101
+ if compliance < 0:
102
+ notes.append(f"compliance clipped from {compliance:.3e}")
103
+ compliance = 0.0
104
+
105
+ # gap: motor travel between the two stalls minus the CAD span (sim only: a CAD span error aliases in)
106
+ gap, gap_observable = 0.0, False
107
+ span = stops.span_rad(joint)
108
+ if kind in ("B1", "B2") and sides[LOWER] and sides[UPPER] and span is not None:
109
+ levels = {
110
+ side: sum(s.motor_raw_rad - side * compliance * s.torque_nm for s in items) / len(items)
111
+ for side, items in sides.items()
112
+ }
113
+ gap = levels[UPPER] - levels[LOWER] - span
114
+ gap_observable = True
115
+ notes.append("gap: stop span vs CAD")
116
+ if gap < 0:
117
+ notes.append(f"gap clipped from {gap:.3e} rad")
118
+ gap = 0.0
119
+
120
+ if compliance_observable:
121
+ model = BacklashModel("B2", gap, compliance)
122
+ elif gap_observable:
123
+ model = BacklashModel("B1", gap)
124
+ else:
125
+ model = BacklashModel("B0")
126
+ return JointBacklash(joint, model, gap_observable, compliance_observable, "identify: " + "; ".join(notes))
@@ -0,0 +1,237 @@
1
+ """The five black-box homing metrics (docs/peps/0002-homing-bench.md), from outputs and truth only (numpy-free stdlib).
2
+
3
+ A homing procedure is scored per episode (one start), never by reading its code:
4
+
5
+ * ``time_to_home_s`` tick clock from the first command to done (median and p95 over finished episodes).
6
+ * ``reliability_pct`` done with no abort, timeout or refusal, and every stall it relied on truly on the CAD stop
7
+ it was meant for (within ``STALL_TOLERANCE_DEG``, bam_fb_test_rig ``stall_proof``'s 1.0 deg).
8
+ Every episode is in the denominator.
9
+ * ``any_start_pct`` share of episodes that are reliable AND have every joint's zero inside the rung's gate.
10
+ * ``backlash_error_deg`` |identified gap - hidden gap| per drive, the worst drive per episode, p95 over reliable
11
+ episodes; a drive not identified is '-' (None), never 0, and ``observable_pct`` says how many
12
+ were. Gate per drive: max(1 arcmin, 25 % of truth).
13
+ * ``joint_angle_error_deg`` |identified zero - true zero| per joint, the worst joint per episode, p95 over reliable
14
+ episodes (reliability holds the failures, so a wrong-stop zero never poses as an accuracy).
15
+
16
+ WHY no weighted sum (report section 2, C's harness): the leaderboard sorts by reliability, then zero error, then time;
17
+ time never buys accuracy. WHY p95 by nearest rank: it is a value an episode actually produced.
18
+ """
19
+
20
+ from __future__ import annotations
21
+
22
+ import math
23
+ import statistics
24
+ from collections.abc import Iterable, Sequence
25
+ from dataclasses import dataclass
26
+
27
+ from robot_msgs import Msg
28
+
29
+ ARCMIN_DEG = 1.0 / 60.0
30
+ STALL_TOLERANCE_DEG = 1.0
31
+ BACKLASH_GATE_FRACTION = 0.25
32
+
33
+
34
+ @dataclass(frozen=True, eq=False, repr=False)
35
+ class StallCheck(Msg):
36
+ """One stall a procedure used as a reference: where the link truly was, against the CAD stop it was meant for."""
37
+
38
+ joint: str
39
+ side: int
40
+ link_deg: float
41
+ cad_deg: float
42
+
43
+ def ok(self, tolerance_deg: float = STALL_TOLERANCE_DEG) -> bool:
44
+ return abs(self.link_deg - self.cad_deg) <= tolerance_deg
45
+
46
+
47
+ @dataclass(frozen=True, eq=False, repr=False)
48
+ class JointOutcome(Msg):
49
+ """One joint after one episode. ``zero_error_deg`` None: the procedure left it unzeroed. ``gap_identified_deg``
50
+ None: it did not identify the gap (shown '-')."""
51
+
52
+ joint: str
53
+ zero_error_deg: float | None
54
+ gap_identified_deg: float | None
55
+ gap_truth_deg: float
56
+
57
+
58
+ @dataclass(frozen=True, eq=False, repr=False)
59
+ class EpisodeRecord(Msg):
60
+ """One procedure from one start in one environment (rung x backlash model)."""
61
+
62
+ procedure: str
63
+ environment: str
64
+ start: str
65
+ finished: bool
66
+ outcome: str
67
+ time_s: float | None
68
+ stalls: tuple[StallCheck, ...]
69
+ joints: tuple[JointOutcome, ...]
70
+
71
+ def worst_zero_error_deg(self) -> float | None:
72
+ errors = [abs(j.zero_error_deg) for j in self.joints if j.zero_error_deg is not None]
73
+ return max(errors) if errors else None
74
+
75
+
76
+ @dataclass(frozen=True)
77
+ class TimeStat:
78
+ median: float
79
+ p95: float
80
+ n: int
81
+
82
+
83
+ @dataclass(frozen=True)
84
+ class ErrorStat:
85
+ p95: float | None
86
+ max: float | None
87
+ median: float | None
88
+ n: int
89
+ missing: int
90
+ worst_joint: str | None
91
+
92
+
93
+ @dataclass(frozen=True)
94
+ class BacklashStat:
95
+ value: float | None
96
+ max: float | None
97
+ observable_pct: float
98
+ n: int
99
+
100
+
101
+ def nearest_rank(values: Iterable[float], percentile: float) -> float:
102
+ """The nearest-rank percentile: the smallest value with at least ``percentile`` % of the values at or below it."""
103
+ ordered = sorted(values)
104
+ if not ordered:
105
+ raise ValueError("nearest_rank of no values")
106
+ rank = max(1, math.ceil(percentile / 100.0 * len(ordered)))
107
+ return ordered[rank - 1]
108
+
109
+
110
+ def reliable(episode: EpisodeRecord, tolerance_deg: float = STALL_TOLERANCE_DEG) -> bool:
111
+ return bool(episode.finished and episode.stalls and all(s.ok(tolerance_deg) for s in episode.stalls))
112
+
113
+
114
+ def reliability_pct(episodes: Sequence[EpisodeRecord]) -> float | None:
115
+ episodes = list(episodes)
116
+ if not episodes:
117
+ return None
118
+ return 100.0 * sum(reliable(e) for e in episodes) / len(episodes)
119
+
120
+
121
+ def time_to_home_s(episodes: Sequence[EpisodeRecord]) -> TimeStat | None:
122
+ times = [e.time_s for e in episodes if e.finished and e.time_s is not None]
123
+ if not times:
124
+ return None
125
+ return TimeStat(statistics.median(times), nearest_rank(times, 95), len(times))
126
+
127
+
128
+ def joint_angle_error_deg(episodes: Sequence[EpisodeRecord]) -> ErrorStat:
129
+ worst, missing, by_joint = [], 0, {}
130
+ for episode in episodes:
131
+ if not reliable(episode):
132
+ continue
133
+ errors = []
134
+ for joint in episode.joints:
135
+ if joint.zero_error_deg is None:
136
+ missing += 1
137
+ continue
138
+ errors.append(abs(joint.zero_error_deg))
139
+ by_joint[joint.joint] = max(by_joint.get(joint.joint, 0.0), abs(joint.zero_error_deg))
140
+ if errors:
141
+ worst.append(max(errors))
142
+ if not worst:
143
+ return ErrorStat(None, None, None, 0, missing, None)
144
+ return ErrorStat(nearest_rank(worst, 95), max(worst), statistics.median(worst), len(worst), missing,
145
+ max(by_joint, key=by_joint.get))
146
+
147
+
148
+ def any_start_pct(episodes: Sequence[EpisodeRecord], gate_deg: float) -> float | None:
149
+ episodes = list(episodes)
150
+ if not episodes:
151
+ return None
152
+
153
+ def inside(episode: EpisodeRecord) -> bool:
154
+ return reliable(episode) and all(
155
+ j.zero_error_deg is not None and abs(j.zero_error_deg) <= gate_deg for j in episode.joints)
156
+
157
+ return 100.0 * sum(inside(e) for e in episodes) / len(episodes)
158
+
159
+
160
+ def backlash_gate_deg(truth_deg: float) -> float:
161
+ return max(ARCMIN_DEG, BACKLASH_GATE_FRACTION * truth_deg)
162
+
163
+
164
+ def backlash_error_deg(episodes: Sequence[EpisodeRecord]) -> BacklashStat:
165
+ worst, identified, total = [], 0, 0
166
+ for episode in episodes:
167
+ if not reliable(episode):
168
+ continue
169
+ errors = []
170
+ for joint in episode.joints:
171
+ total += 1
172
+ if joint.gap_identified_deg is None:
173
+ continue
174
+ identified += 1
175
+ errors.append(abs(joint.gap_identified_deg - joint.gap_truth_deg))
176
+ if errors:
177
+ worst.append(max(errors))
178
+ observable = 100.0 * identified / total if total else 0.0
179
+ if not worst:
180
+ return BacklashStat(None, None, observable, 0)
181
+ return BacklashStat(nearest_rank(worst, 95), max(worst), observable, len(worst))
182
+
183
+
184
+ def backlash_gate_pass(episodes: Sequence[EpisodeRecord]) -> bool | None:
185
+ verdicts = [abs(j.gap_identified_deg - j.gap_truth_deg) <= backlash_gate_deg(j.gap_truth_deg)
186
+ for e in episodes if reliable(e) for j in e.joints if j.gap_identified_deg is not None]
187
+ return all(verdicts) if verdicts else None
188
+
189
+
190
+ def scorecard(episodes: Sequence[EpisodeRecord], *, gate_deg: float, reliability_target_pct: float = 100.0) -> dict:
191
+ """The five metrics plus the rung's verdicts, flat (a W&B summary / leaderboard row)."""
192
+ episodes = list(episodes)
193
+ time = time_to_home_s(episodes)
194
+ zero = joint_angle_error_deg(episodes)
195
+ gap = backlash_error_deg(episodes)
196
+ reliability = reliability_pct(episodes)
197
+ return {
198
+ "episodes": len(episodes),
199
+ "time_to_home_s": None if time is None else time.median,
200
+ "time_to_home_p95_s": None if time is None else time.p95,
201
+ "reliability_pct": reliability,
202
+ "any_start_pct": any_start_pct(episodes, gate_deg),
203
+ "backlash_error_deg": gap.value,
204
+ "backlash_error_max_deg": gap.max,
205
+ "backlash_observable_pct": gap.observable_pct,
206
+ "joint_angle_error_deg": zero.p95,
207
+ "joint_angle_error_max_deg": zero.max,
208
+ "joint_angle_error_median_deg": zero.median,
209
+ "joint_angle_error_worst_joint": zero.worst_joint,
210
+ "joints_unzeroed": zero.missing,
211
+ "zero_gate_deg": gate_deg,
212
+ "zero_gate_pass": None if zero.max is None else bool(zero.max <= gate_deg and zero.missing == 0),
213
+ "reliability_gate_pass": None if reliability is None else bool(reliability >= reliability_target_pct),
214
+ "backlash_gate_pass": backlash_gate_pass(episodes),
215
+ }
216
+
217
+
218
+ __all__ = [
219
+ "ARCMIN_DEG",
220
+ "STALL_TOLERANCE_DEG",
221
+ "BacklashStat",
222
+ "EpisodeRecord",
223
+ "ErrorStat",
224
+ "JointOutcome",
225
+ "StallCheck",
226
+ "TimeStat",
227
+ "any_start_pct",
228
+ "backlash_error_deg",
229
+ "backlash_gate_deg",
230
+ "backlash_gate_pass",
231
+ "joint_angle_error_deg",
232
+ "nearest_rank",
233
+ "reliability_pct",
234
+ "reliable",
235
+ "scorecard",
236
+ "time_to_home_s",
237
+ ]
@@ -0,0 +1,72 @@
1
+ """The homing bench's two seams (docs/peps/0002-homing-bench.md; the decision report's proposal C, grafted onto B).
2
+
3
+ * ``HomingPlant`` what a procedure runs on: ``reset(start)``, ``tick(command, dt)``, ``truth()``. A kinematic stop plant,
4
+ a MuJoCo rig and the real bus implement it, so the SAME procedure, the same sequence bytes, runs on every
5
+ rung; only the plant changes.
6
+ * ``HomingProcedure`` a black box: ``plan()`` (collect) emits a robot_msgs ``JointCommandSequence`` (or None when done),
7
+ run by the ``JointCommandSequenceController``; ``observe()`` (fit) turns what came back into a
8
+ ``HomingResult``. Nothing in the bench reads a procedure's internals.
9
+
10
+ Typing only: no runtime dependency beyond robot_msgs and this package.
11
+ """
12
+
13
+ from __future__ import annotations
14
+
15
+ from collections.abc import Mapping, Sequence
16
+ from dataclasses import dataclass, field
17
+ from typing import Any, Protocol, runtime_checkable
18
+
19
+ from robot_msgs.control import JointCommandSequence, ServoCommand
20
+ from robot_msgs.sensor import JointState
21
+
22
+ from robot_models.homing.types import BacklashTable, HomingResult, StopMap
23
+
24
+
25
+ @runtime_checkable
26
+ class HomingPlant(Protocol):
27
+ """A rig a homing sequence can run on, one controller tick at a time."""
28
+
29
+ joint_names: tuple[str, ...]
30
+
31
+ def reset(self, start: Any) -> JointState: ...
32
+
33
+ def tick(self, command: ServoCommand, dt: float, defines: Mapping[str, float] | None = None) -> JointState: ...
34
+
35
+ def truth(self) -> Mapping[str, Any]: ...
36
+
37
+
38
+ @dataclass(frozen=True)
39
+ class Prior:
40
+ """What a procedure may know before it moves: the stop map (CAD or typed), the stored backlash table (None on a
41
+ first boot) and the joints it homes, each with the side it seeks first."""
42
+
43
+ stops: StopMap
44
+ joints: tuple[str, ...]
45
+ seek_side: Mapping[str, int]
46
+ table: BacklashTable | None = None
47
+ caps_nm: Mapping[str, float] = field(default_factory=dict)
48
+
49
+
50
+ @dataclass(frozen=True)
51
+ class Phase:
52
+ """One sequence as it ran: the sequence, the controller's step events and the per-tick log the plant measured."""
53
+
54
+ sequence: JointCommandSequence
55
+ status: str
56
+ message: str
57
+ events: tuple[Any, ...]
58
+ log: Mapping[str, Any]
59
+
60
+
61
+ @runtime_checkable
62
+ class HomingProcedure(Protocol):
63
+ """A homing procedure as a black box: collect (``plan``) then fit (``observe``)."""
64
+
65
+ name: str
66
+
67
+ def plan(self, prior: Prior, so_far: Sequence[Phase]) -> JointCommandSequence | None: ...
68
+
69
+ def observe(self, prior: Prior, phases: Sequence[Phase]) -> HomingResult: ...
70
+
71
+
72
+ __all__ = ["HomingPlant", "HomingProcedure", "Phase", "Prior"]