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.
- robot_models/__init__.py +18 -0
- robot_models/actuators/__init__.py +1 -0
- robot_models/actuators/rungs/README.md +24 -0
- robot_models/actuators/rungs/coulomb_viscous/README.md +61 -0
- robot_models/actuators/rungs/coulomb_viscous/model.js +14 -0
- robot_models/actuators/rungs/coulomb_viscous/model.json +102 -0
- robot_models/actuators/rungs/coulomb_viscous/model.py +24 -0
- robot_models/actuators/rungs/datasheet/README.md +65 -0
- robot_models/actuators/rungs/datasheet/model.js +14 -0
- robot_models/actuators/rungs/datasheet/model.json +138 -0
- robot_models/actuators/rungs/datasheet/model.py +24 -0
- robot_models/actuators/rungs/dcmotor/README.md +63 -0
- robot_models/actuators/rungs/dcmotor/model.json +163 -0
- robot_models/actuators/rungs/ideal/README.md +60 -0
- robot_models/actuators/rungs/ideal/model.js +6 -0
- robot_models/actuators/rungs/ideal/model.json +90 -0
- robot_models/actuators/rungs/ideal/model.py +13 -0
- robot_models/actuators/rungs/load/README.md +65 -0
- robot_models/actuators/rungs/load/model.js +15 -0
- robot_models/actuators/rungs/load/model.json +117 -0
- robot_models/actuators/rungs/load/model.py +25 -0
- robot_models/actuators/rungs/lugre/README.md +72 -0
- robot_models/actuators/rungs/lugre/model.js +24 -0
- robot_models/actuators/rungs/lugre/model.json +164 -0
- robot_models/actuators/rungs/lugre/model.py +30 -0
- robot_models/actuators/rungs/ripple/README.md +64 -0
- robot_models/actuators/rungs/ripple/model.js +17 -0
- robot_models/actuators/rungs/ripple/model.json +119 -0
- robot_models/actuators/rungs/ripple/model.py +27 -0
- robot_models/actuators/rungs/stribeck/README.md +65 -0
- robot_models/actuators/rungs/stribeck/model.js +17 -0
- robot_models/actuators/rungs/stribeck/model.json +126 -0
- robot_models/actuators/rungs/stribeck/model.py +28 -0
- robot_models/actuators/servo/README.md +278 -0
- robot_models/actuators/servo/__init__.py +7 -0
- robot_models/actuators/servo/bldc/README.md +186 -0
- robot_models/actuators/servo/bldc/__init__.py +5 -0
- robot_models/actuators/servo/bldc/bldc_motor.py +93 -0
- robot_models/actuators/servo/bldc/ladder.py +268 -0
- robot_models/actuators/servo/foc_controller/README.md +127 -0
- robot_models/actuators/servo/foc_controller/__init__.py +3 -0
- robot_models/actuators/servo/foc_controller/ladder.py +207 -0
- robot_models/actuators/servo/gearbox/README.md +148 -0
- robot_models/actuators/servo/gearbox/__init__.py +5 -0
- robot_models/actuators/servo/gearbox/gearbox.py +36 -0
- robot_models/actuators/servo/gearbox/ladder.py +214 -0
- robot_models/actuators/servo/integrated_servo.py +33 -0
- robot_models/actuators/servo/ladder.py +593 -0
- robot_models/arms/__init__.py +1 -0
- robot_models/arms/rigid_body_chain/README.md +151 -0
- robot_models/arms/rigid_body_chain/__init__.py +3 -0
- robot_models/arms/rigid_body_chain/ladder.py +228 -0
- robot_models/conveyors/__init__.py +1 -0
- robot_models/conveyors/conveyor/README.md +140 -0
- robot_models/conveyors/conveyor/__init__.py +1 -0
- robot_models/conveyors/conveyor/ladder.py +203 -0
- robot_models/datasheet/__init__.py +56 -0
- robot_models/datasheet/datasheet.py +355 -0
- robot_models/datasheet/literal.py +144 -0
- robot_models/datasheet/source.py +328 -0
- robot_models/datasheet/stated.py +135 -0
- robot_models/datasheet/units.py +57 -0
- robot_models/end_effectors/__init__.py +1 -0
- robot_models/end_effectors/gripper/README.md +112 -0
- robot_models/end_effectors/gripper/__init__.py +3 -0
- robot_models/end_effectors/gripper/ladder.py +150 -0
- robot_models/homing/__init__.py +38 -0
- robot_models/homing/cad_bundle.py +206 -0
- robot_models/homing/contact.py +201 -0
- robot_models/homing/fit.py +126 -0
- robot_models/homing/metrics.py +237 -0
- robot_models/homing/protocols.py +72 -0
- robot_models/homing/types.py +256 -0
- robot_models/ladder.py +645 -0
- robot_models/markers/__init__.py +1 -0
- robot_models/markers/aruco_marker/README.md +92 -0
- robot_models/markers/aruco_marker/__init__.py +2 -0
- robot_models/markers/aruco_marker/ladder.py +142 -0
- robot_models/references.py +429 -0
- robot_models/sensors/__init__.py +1 -0
- robot_models/sensors/camera/README.md +118 -0
- robot_models/sensors/camera/__init__.py +3 -0
- robot_models/sensors/camera/ladder.py +108 -0
- robot_models/sensors/encoder/README.md +103 -0
- robot_models/sensors/encoder/__init__.py +5 -0
- robot_models/sensors/encoder/ladder.py +133 -0
- robot_models/sensors/hyperspectral_camera/README.md +107 -0
- robot_models/sensors/hyperspectral_camera/__init__.py +1 -0
- robot_models/sensors/hyperspectral_camera/ladder.py +151 -0
- robot_models/sensors/rgbd/README.md +113 -0
- robot_models/sensors/rgbd/__init__.py +5 -0
- robot_models/sensors/rgbd/ladder.py +113 -0
- robot_models_lib-0.2.0.dist-info/METADATA +143 -0
- robot_models_lib-0.2.0.dist-info/RECORD +96 -0
- robot_models_lib-0.2.0.dist-info/WHEEL +4 -0
- 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"]
|