bounded-embodiment-kit 0.1.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.
- bounded_embodiment_kit/__init__.py +2 -0
- bounded_embodiment_kit/adapters/__init__.py +29 -0
- bounded_embodiment_kit/adapters/base.py +192 -0
- bounded_embodiment_kit/adapters/feetech.py +204 -0
- bounded_embodiment_kit/adapters/lerobot.py +316 -0
- bounded_embodiment_kit/adapters/python.py +136 -0
- bounded_embodiment_kit/adapters/sim_rover.py +206 -0
- bounded_embodiment_kit/adapters/sim_so101.py +615 -0
- bounded_embodiment_kit/cli.py +23 -0
- bounded_embodiment_kit/instruments/__init__.py +1 -0
- bounded_embodiment_kit/instruments/accuracy.py +866 -0
- bounded_embodiment_kit/instruments/markerless/__init__.py +16 -0
- bounded_embodiment_kit/instruments/markerless/background.py +123 -0
- bounded_embodiment_kit/instruments/markerless/cli.py +319 -0
- bounded_embodiment_kit/instruments/markerless/fit.py +1346 -0
- bounded_embodiment_kit/instruments/markerless/mesh.py +253 -0
- bounded_embodiment_kit/instruments/markerless/model.py +316 -0
- bounded_embodiment_kit/instruments/markerless/register.py +368 -0
- bounded_embodiment_kit/instruments/markerless/synth.py +669 -0
- bounded_embodiment_kit/instruments/plan.py +279 -0
- bounded_embodiment_kit/instruments/record.py +2481 -0
- bounded_embodiment_kit/instruments/session.py +291 -0
- bounded_embodiment_kit/kinematics/__init__.py +5 -0
- bounded_embodiment_kit/kinematics/ik.py +109 -0
- bounded_embodiment_kit/kinematics/urdf.py +401 -0
- bounded_embodiment_kit/runner/__init__.py +4 -0
- bounded_embodiment_kit/runner/cli.py +170 -0
- bounded_embodiment_kit/runner/doctor.py +128 -0
- bounded_embodiment_kit/runner/feeds.py +315 -0
- bounded_embodiment_kit/runner/interlocks.py +122 -0
- bounded_embodiment_kit/runner/kin.py +75 -0
- bounded_embodiment_kit/runner/limits.py +116 -0
- bounded_embodiment_kit/runner/run.py +701 -0
- bounded_embodiment_kit/runner/session.py +422 -0
- bounded_embodiment_kit/runner/watchdog.py +484 -0
- bounded_embodiment_kit/submission/__init__.py +11 -0
- bounded_embodiment_kit/submission/cli.py +96 -0
- bounded_embodiment_kit/submission/files.py +127 -0
- bounded_embodiment_kit/submission/ids.py +91 -0
- bounded_embodiment_kit/submission/lab-config-0.1.json +50 -0
- bounded_embodiment_kit/submission/labconfig.py +77 -0
- bounded_embodiment_kit/submission/report.py +733 -0
- bounded_embodiment_kit/submission/scrub.py +342 -0
- bounded_embodiment_kit/submission/submit.py +328 -0
- bounded_embodiment_kit/submission/timebase.py +396 -0
- bounded_embodiment_kit-0.1.0.dist-info/METADATA +39 -0
- bounded_embodiment_kit-0.1.0.dist-info/RECORD +49 -0
- bounded_embodiment_kit-0.1.0.dist-info/WHEEL +4 -0
- bounded_embodiment_kit-0.1.0.dist-info/entry_points.txt +2 -0
|
@@ -0,0 +1,316 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Craig Merry and contributors
|
|
2
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
3
|
+
"""DRAFT — requires line-by-line human review before use on hardware.
|
|
4
|
+
|
|
5
|
+
The ``lerobot`` adapter: a LeRobot SO-100 / SO-101 follower arm on a Feetech bus.
|
|
6
|
+
|
|
7
|
+
How a lab's gate plugs in. The adapter builds LeRobot's follower and connects its bus ITSELF with
|
|
8
|
+
``feetech.connect_safely`` (goals synced to present before any torque). Only then does it hand the
|
|
9
|
+
already-connected robot to the lab's ``--wrapper module:factory``, called as
|
|
10
|
+
``factory(robot, context)``; the factory returns the gated wrapper (anything with
|
|
11
|
+
``send_action(action) -> dict``). From then on:
|
|
12
|
+
|
|
13
|
+
- every command goes through ``wrapper.send_action`` (the system under test);
|
|
14
|
+
- the adapter NEVER calls the wrapper's ``connect()``, ``configure()`` or ``disconnect()``, nor the
|
|
15
|
+
inner robot's ``connect()`` or ``disconnect()``: LeRobot's ``connect()`` enables torque toward
|
|
16
|
+
goal registers that read 0 at power-up, and its ``disconnect()`` drops torque by default
|
|
17
|
+
(``SOFollowerConfig.disable_torque_on_disconnect=True``), which drops the arm;
|
|
18
|
+
- stops, state reads and the final release go straight to the inner robot's bus, under the bus
|
|
19
|
+
lock, so a gate that misbehaves cannot block or alter a stop (it can only delay it by holding
|
|
20
|
+
the bus inside send_action; the watchdog's acknowledgement timer covers that case).
|
|
21
|
+
|
|
22
|
+
``context`` is plain data: ``mode``, ``speed_factor`` (0.5 in a shakedown, else None: "use your own
|
|
23
|
+
default"), and the paths of the envelope, command space, kit config and URDF.
|
|
24
|
+
|
|
25
|
+
Other LeRobot robots (Dynamixel, Damiao, Robstride, Reachy 2 buses) are refused in v0.1: their
|
|
26
|
+
connect path and their stop differ. A lab with one writes a ``python`` adapter with its own connect
|
|
27
|
+
path, and runs with ``--estop-cmd``.
|
|
28
|
+
|
|
29
|
+
LeRobot is imported only when a real robot is built (``build_follower``), never at import time.
|
|
30
|
+
"""
|
|
31
|
+
from __future__ import annotations
|
|
32
|
+
|
|
33
|
+
import importlib
|
|
34
|
+
import sys
|
|
35
|
+
import threading
|
|
36
|
+
from collections.abc import Callable
|
|
37
|
+
from pathlib import Path
|
|
38
|
+
|
|
39
|
+
from bounded_embodiment.commandspace import CommandSpace, load_command_space
|
|
40
|
+
|
|
41
|
+
from .base import AdapterError, SendResult, State, clean_text, load_callable
|
|
42
|
+
from .feetech import (
|
|
43
|
+
BODY_JOINT_SPEED_TRIP_DPS,
|
|
44
|
+
check_bus_is_ours,
|
|
45
|
+
choose_stop_goals,
|
|
46
|
+
connect_safely,
|
|
47
|
+
leave_safe,
|
|
48
|
+
norm_name,
|
|
49
|
+
)
|
|
50
|
+
|
|
51
|
+
__all__ = ["SUPPORTED_ROBOT_TYPES", "LeRobotAdapter", "add_arguments", "build_follower", "from_args",
|
|
52
|
+
"lerobot_version"]
|
|
53
|
+
|
|
54
|
+
SUPPORTED_ROBOT_TYPES = ("so101_follower", "so100_follower")
|
|
55
|
+
|
|
56
|
+
# LeRobot has moved the follower classes between releases (ev03-harness lerobot_compat.py).
|
|
57
|
+
_FOLLOWER_PATHS = [
|
|
58
|
+
("lerobot.robots.so_follower", "SO101Follower", "SO101FollowerConfig"), # 0.6.x, main
|
|
59
|
+
("lerobot.robots.so101_follower", "SO101Follower", "SO101FollowerConfig"), # 0.2 - 0.4
|
|
60
|
+
("lerobot.common.robots.so101_follower", "SO101Follower", "SO101FollowerConfig"),
|
|
61
|
+
]
|
|
62
|
+
|
|
63
|
+
#: How long close() waits for the bus lock before giving up (the command thread may be stuck
|
|
64
|
+
#: inside a gate's send_action, holding the bus).
|
|
65
|
+
CLOSE_LOCK_TIMEOUT_S = 5.0
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
def _log(msg: str) -> None:
|
|
69
|
+
print(msg, file=sys.stderr, flush=True)
|
|
70
|
+
|
|
71
|
+
|
|
72
|
+
def lerobot_version() -> str:
|
|
73
|
+
try:
|
|
74
|
+
from importlib.metadata import version
|
|
75
|
+
|
|
76
|
+
return version("lerobot")
|
|
77
|
+
except Exception: # noqa: BLE001 - not installed, or broken metadata: say so, never fail
|
|
78
|
+
return "not installed"
|
|
79
|
+
|
|
80
|
+
|
|
81
|
+
def build_follower(robot_type: str, port: str, robot_id: str,
|
|
82
|
+
max_relative_target: float | None = None) -> object:
|
|
83
|
+
"""Build (never connect) LeRobot's SO-10x follower, exactly as LeRobot's own scripts would."""
|
|
84
|
+
if robot_type not in SUPPORTED_ROBOT_TYPES:
|
|
85
|
+
raise AdapterError(f"robot type {robot_type!r} is refused in v0.1: only Feetech SO-10x "
|
|
86
|
+
f"followers {SUPPORTED_ROBOT_TYPES} have a reviewed connect path and stop. "
|
|
87
|
+
"Write a python adapter (be adapter new) with your own connect path, and "
|
|
88
|
+
"run with --estop-cmd.")
|
|
89
|
+
errors = []
|
|
90
|
+
for module, robot_cls, config_cls in _FOLLOWER_PATHS:
|
|
91
|
+
try:
|
|
92
|
+
m = importlib.import_module(module)
|
|
93
|
+
robot_class, config_class = getattr(m, robot_cls), getattr(m, config_cls)
|
|
94
|
+
break
|
|
95
|
+
except (ImportError, AttributeError) as e:
|
|
96
|
+
errors.append(f"{module}: {e}")
|
|
97
|
+
else:
|
|
98
|
+
raise AdapterError("LeRobot's SO-101 follower was not found (pip install "
|
|
99
|
+
"'bounded-embodiment-kit[lerobot]', Python >= 3.12). Tried:\n "
|
|
100
|
+
+ "\n ".join(errors))
|
|
101
|
+
kwargs = {"port": port, "id": robot_id, "max_relative_target": max_relative_target}
|
|
102
|
+
try:
|
|
103
|
+
cfg = config_class(use_degrees=True, **kwargs)
|
|
104
|
+
except TypeError: # older LeRobot had no use_degrees option (connect_safely then refuses)
|
|
105
|
+
cfg = config_class(**kwargs)
|
|
106
|
+
return robot_class(cfg)
|
|
107
|
+
|
|
108
|
+
|
|
109
|
+
class LeRobotAdapter:
|
|
110
|
+
"""The adapter contract (``adapters.base``) for a LeRobot SO-10x follower."""
|
|
111
|
+
|
|
112
|
+
name = "lerobot"
|
|
113
|
+
simulation = False
|
|
114
|
+
|
|
115
|
+
def __init__(self, *, command_space_path: str | Path, port: str, robot_id: str,
|
|
116
|
+
robot_type: str = "so101_follower", wrapper: str | None = None,
|
|
117
|
+
wrapper_context: dict | None = None, max_relative_target: float | None = None,
|
|
118
|
+
refuse_while_active: tuple[str, ...] = (),
|
|
119
|
+
prompt: Callable[[str], str] = input, log: Callable[[str], None] = _log,
|
|
120
|
+
make_robot: Callable[[], object] | None = None):
|
|
121
|
+
self.command_space_path = Path(command_space_path)
|
|
122
|
+
self.port, self.robot_id, self.robot_type = port, robot_id, robot_type
|
|
123
|
+
self.wrapper = wrapper
|
|
124
|
+
self.wrapper_context = dict(wrapper_context or {})
|
|
125
|
+
self.max_relative_target = max_relative_target
|
|
126
|
+
self.refuse_while_active = tuple(refuse_while_active)
|
|
127
|
+
self.prompt, self.log = prompt, log
|
|
128
|
+
self._make_robot = make_robot or (lambda: build_follower(robot_type, port, robot_id,
|
|
129
|
+
max_relative_target))
|
|
130
|
+
# One bus lock. Every bus access takes it: send, read_state, execute_stop, close. The
|
|
131
|
+
# watchdog never takes it; latch_stop() only sets an Event.
|
|
132
|
+
self._bus_lock = threading.RLock()
|
|
133
|
+
self._latched = threading.Event()
|
|
134
|
+
self._stop_goals: dict[str, int] | None = None
|
|
135
|
+
self._robot = None
|
|
136
|
+
self._target = None
|
|
137
|
+
self._connect_record: dict | None = None
|
|
138
|
+
|
|
139
|
+
# ----- open --------------------------------------------------------------------------------
|
|
140
|
+
def open(self) -> CommandSpace:
|
|
141
|
+
# The command space comes from the lab's declared file (URDF limits), never from bus
|
|
142
|
+
# calibration: bob's elbow and wrist are calibrated to the full encoder range.
|
|
143
|
+
space = load_command_space(self.command_space_path)
|
|
144
|
+
if space.kind != "joint_position":
|
|
145
|
+
raise AdapterError(f"the lerobot adapter drives joint positions; {self.command_space_path} "
|
|
146
|
+
f"declares {space.kind!r}")
|
|
147
|
+
# Import the lab's factory BEFORE any torque, so an import error cannot leave a powered arm.
|
|
148
|
+
factory = load_callable(self.wrapper) if self.wrapper else None
|
|
149
|
+
self._preflight()
|
|
150
|
+
robot = self._make_robot()
|
|
151
|
+
self._check_robot(robot, space)
|
|
152
|
+
with self._bus_lock:
|
|
153
|
+
self._connect_record = connect_safely(robot, log=self.log)
|
|
154
|
+
self._robot = robot
|
|
155
|
+
try:
|
|
156
|
+
target = robot
|
|
157
|
+
if factory is not None:
|
|
158
|
+
target = factory(robot, dict(self.wrapper_context))
|
|
159
|
+
if not callable(getattr(target, "send_action", None)):
|
|
160
|
+
raise AdapterError(f"{self.wrapper} returned {type(target).__name__}, which has "
|
|
161
|
+
"no send_action(action)")
|
|
162
|
+
except BaseException:
|
|
163
|
+
self._release_without_prompt()
|
|
164
|
+
raise
|
|
165
|
+
self._target = target
|
|
166
|
+
self._stop_goals = None
|
|
167
|
+
self._latched.clear() # a new latch for this connection
|
|
168
|
+
return space
|
|
169
|
+
|
|
170
|
+
def _preflight(self) -> None:
|
|
171
|
+
"""Refuse if anything else could command the arm (hw.py check_bus_is_ours)."""
|
|
172
|
+
problems = check_bus_is_ours(self.port, self.refuse_while_active)
|
|
173
|
+
if problems:
|
|
174
|
+
raise AdapterError("Something else could command the arm:\n " + "\n ".join(problems))
|
|
175
|
+
|
|
176
|
+
def _check_robot(self, robot: object, space: CommandSpace) -> None:
|
|
177
|
+
bus = getattr(robot, "bus", None)
|
|
178
|
+
motors = getattr(bus, "motors", None)
|
|
179
|
+
if not motors:
|
|
180
|
+
raise AdapterError(f"{type(robot).__name__} has no bus with motors")
|
|
181
|
+
unknown = [a.key for a in space.axes
|
|
182
|
+
if not (a.key.endswith(".pos") and a.key[: -len(".pos")] in motors)]
|
|
183
|
+
if unknown:
|
|
184
|
+
raise AdapterError(f"command-space axes {unknown} are not '<motor>.pos' for this "
|
|
185
|
+
f"robot's motors {list(motors)}")
|
|
186
|
+
|
|
187
|
+
def _release_without_prompt(self) -> None:
|
|
188
|
+
robot, self._robot, self._target = self._robot, None, None
|
|
189
|
+
if robot is None:
|
|
190
|
+
return
|
|
191
|
+
try:
|
|
192
|
+
with self._bus_lock:
|
|
193
|
+
if getattr(robot.bus, "is_connected", False):
|
|
194
|
+
robot.bus.disconnect(disable_torque=False) # torque untouched: the arm holds
|
|
195
|
+
except Exception as e: # noqa: BLE001 - already failing; report and keep the first error
|
|
196
|
+
self.log(f"Could not release the port ({type(e).__name__}: {e}).")
|
|
197
|
+
|
|
198
|
+
# ----- describe ----------------------------------------------------------------------------
|
|
199
|
+
def describe(self) -> dict:
|
|
200
|
+
motors = list(getattr(getattr(self._robot, "bus", None), "motors", {}) or {})
|
|
201
|
+
body = [m for m in motors if m != "gripper"]
|
|
202
|
+
return {
|
|
203
|
+
"adapter": self.name,
|
|
204
|
+
"simulation": self.simulation,
|
|
205
|
+
# describe()["config"] is what the setup hash covers: everything that changes what is
|
|
206
|
+
# being tested or how it is driven, and nothing that changes from run to run.
|
|
207
|
+
"config": {
|
|
208
|
+
"adapter": self.name,
|
|
209
|
+
"robot_type": self.robot_type,
|
|
210
|
+
"port": self.port,
|
|
211
|
+
"robot_id": self.robot_id,
|
|
212
|
+
"wrapper": self.wrapper,
|
|
213
|
+
"max_relative_target": self.max_relative_target,
|
|
214
|
+
},
|
|
215
|
+
"lerobot_version": lerobot_version(),
|
|
216
|
+
"connect": self._connect_record,
|
|
217
|
+
"stop": {"rule": "every joint's present position, read at the first stop of a latch "
|
|
218
|
+
"and re-sent unchanged by every later stop; raw ticks written to the "
|
|
219
|
+
"inner bus, not through the wrapper"},
|
|
220
|
+
"joint_speed_trip_per_s": {m: BODY_JOINT_SPEED_TRIP_DPS for m in body},
|
|
221
|
+
"refuse_while_active": list(self.refuse_while_active),
|
|
222
|
+
}
|
|
223
|
+
|
|
224
|
+
# ----- commands ----------------------------------------------------------------------------
|
|
225
|
+
def send(self, cmd: dict) -> SendResult:
|
|
226
|
+
"""Hand one proposal to the system under test. Never raises: the hostile stream sends NaN,
|
|
227
|
+
strings and missing keys on purpose, and LeRobot or a gate raising on them is a result."""
|
|
228
|
+
if self._latched.is_set():
|
|
229
|
+
return SendResult(False, None, "stop_latched")
|
|
230
|
+
target = self._target
|
|
231
|
+
if target is None:
|
|
232
|
+
return SendResult(False, None, "not_open")
|
|
233
|
+
try:
|
|
234
|
+
with self._bus_lock:
|
|
235
|
+
if self._latched.is_set(): # re-checked under the lock: the stop wins any race
|
|
236
|
+
return SendResult(False, None, "stop_latched")
|
|
237
|
+
target.send_action(cmd)
|
|
238
|
+
except Exception as e: # noqa: BLE001 - send() never raises; the error is the result
|
|
239
|
+
return SendResult(False, None, f"exception:{type(e).__name__}")
|
|
240
|
+
decision = getattr(target, "last_decision", None) if target is not self._robot else None
|
|
241
|
+
return SendResult(True, clean_text(decision), None)
|
|
242
|
+
|
|
243
|
+
def read_state(self) -> State | None:
|
|
244
|
+
robot = self._robot
|
|
245
|
+
if robot is None:
|
|
246
|
+
return None
|
|
247
|
+
with self._bus_lock:
|
|
248
|
+
values = robot.bus.sync_read("Present_Position")
|
|
249
|
+
units = {m: ("deg" if norm_name(motor) == "DEGREES" else "percent")
|
|
250
|
+
for m, motor in robot.bus.motors.items()}
|
|
251
|
+
return State(joints={m: float(v) for m, v in values.items()}, units=units)
|
|
252
|
+
|
|
253
|
+
# ----- the stop ----------------------------------------------------------------------------
|
|
254
|
+
def latch_stop(self) -> None:
|
|
255
|
+
"""Any thread. Sets the latch and returns at once: no bus access, no lock."""
|
|
256
|
+
self._latched.set()
|
|
257
|
+
|
|
258
|
+
def execute_stop(self) -> None:
|
|
259
|
+
"""Command thread only. The stop: the present positions read at the first stop of this
|
|
260
|
+
latch (``feetech.choose_stop_goals``), written straight to the inner bus in raw ticks (no
|
|
261
|
+
wrapper, no LeRobot clamp, no degree rounding). Every later call re-sends exactly those."""
|
|
262
|
+
self._latched.set()
|
|
263
|
+
robot = self._robot
|
|
264
|
+
if robot is None:
|
|
265
|
+
raise AdapterError("execute_stop: the robot is not open")
|
|
266
|
+
with self._bus_lock:
|
|
267
|
+
bus = robot.bus
|
|
268
|
+
if self._stop_goals is None:
|
|
269
|
+
present = bus.sync_read("Present_Position", normalize=False)
|
|
270
|
+
self._stop_goals = choose_stop_goals(present)
|
|
271
|
+
self.log("Stop: every joint held where it was; later stops re-send these goals.")
|
|
272
|
+
bus.sync_write("Goal_Position", dict(self._stop_goals), normalize=False)
|
|
273
|
+
|
|
274
|
+
# ----- close -------------------------------------------------------------------------------
|
|
275
|
+
def close(self) -> None:
|
|
276
|
+
"""Leave the arm holding (torque on) unless the operator types OFF; release the port. Never
|
|
277
|
+
through LeRobot's robot.disconnect() or the wrapper's."""
|
|
278
|
+
robot = self._robot
|
|
279
|
+
if robot is None:
|
|
280
|
+
return
|
|
281
|
+
if not self._bus_lock.acquire(timeout=CLOSE_LOCK_TIMEOUT_S):
|
|
282
|
+
self.log("*** The bus is still busy (a command is stuck?). Not touching it. "
|
|
283
|
+
"CUT SERVO POWER if the arm is not holding still.")
|
|
284
|
+
return
|
|
285
|
+
try:
|
|
286
|
+
leave_safe(robot.bus, self.prompt, self.log)
|
|
287
|
+
finally:
|
|
288
|
+
self._bus_lock.release()
|
|
289
|
+
self._robot, self._target = None, None
|
|
290
|
+
|
|
291
|
+
|
|
292
|
+
# ----- command line ----------------------------------------------------------------------------
|
|
293
|
+
def add_arguments(p) -> None:
|
|
294
|
+
g = p.add_argument_group("lerobot adapter")
|
|
295
|
+
g.add_argument("--port", help="the follower's serial port (lerobot)")
|
|
296
|
+
g.add_argument("--id", dest="robot_id", help="the LeRobot robot id (its calibration file)")
|
|
297
|
+
g.add_argument("--robot-type", default="so101_follower", choices=SUPPORTED_ROBOT_TYPES)
|
|
298
|
+
g.add_argument("--wrapper", metavar="MODULE:FACTORY",
|
|
299
|
+
help="the lab's gate: factory(robot, context) -> wrapper with send_action "
|
|
300
|
+
"(module:name or path/to/file.py:name); omit for the bare robot")
|
|
301
|
+
g.add_argument("--max-relative-target", type=float, default=None,
|
|
302
|
+
help="LeRobot's own per-command step clamp (part of the system under test)")
|
|
303
|
+
g.add_argument("--refuse-while-active", action="append", default=[], metavar="UNIT",
|
|
304
|
+
help="a systemd unit that can also drive the arm; refuse while it is active")
|
|
305
|
+
|
|
306
|
+
|
|
307
|
+
def from_args(args, context) -> LeRobotAdapter:
|
|
308
|
+
if not args.port or not args.robot_id:
|
|
309
|
+
raise AdapterError("the lerobot adapter needs --port and --id")
|
|
310
|
+
return LeRobotAdapter(command_space_path=context.command_space_path, port=args.port,
|
|
311
|
+
robot_id=args.robot_id, robot_type=args.robot_type,
|
|
312
|
+
wrapper=getattr(args, "wrapper", None),
|
|
313
|
+
wrapper_context=context.wrapper_context(),
|
|
314
|
+
max_relative_target=args.max_relative_target,
|
|
315
|
+
refuse_while_active=tuple(args.refuse_while_active),
|
|
316
|
+
prompt=context.prompt, log=context.log)
|
|
@@ -0,0 +1,136 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Craig Merry and contributors
|
|
2
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
3
|
+
"""DRAFT — requires line-by-line human review before use on hardware.
|
|
4
|
+
|
|
5
|
+
The ``python`` adapter: a lab's own adapter class, loaded by name (``--adapter-class
|
|
6
|
+
path/to/my_adapter.py:Adapter`` or ``module:Class``) and constructed as ``Class(context)`` with
|
|
7
|
+
the run's ``AdapterContext``. ``be adapter new NAME`` writes a starting point (``TEMPLATE``).
|
|
8
|
+
|
|
9
|
+
The runner checks that the object has every method of the contract (``adapters.base``) before it
|
|
10
|
+
calls any of them.
|
|
11
|
+
"""
|
|
12
|
+
from __future__ import annotations
|
|
13
|
+
|
|
14
|
+
from pathlib import Path
|
|
15
|
+
|
|
16
|
+
from .base import AdapterError, check_adapter, load_callable
|
|
17
|
+
|
|
18
|
+
__all__ = ["TEMPLATE", "add_arguments", "from_args", "write_template"]
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
def add_arguments(p) -> None:
|
|
22
|
+
g = p.add_argument_group("python adapter")
|
|
23
|
+
g.add_argument("--adapter-class", metavar="FILE.py:CLASS",
|
|
24
|
+
help="the lab's adapter class (python adapter), constructed as Class(context)")
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
def from_args(args, context):
|
|
28
|
+
if not getattr(args, "adapter_class", None):
|
|
29
|
+
raise AdapterError("the python adapter needs --adapter-class FILE.py:CLASS (be adapter new "
|
|
30
|
+
"NAME writes a template)")
|
|
31
|
+
cls = load_callable(args.adapter_class)
|
|
32
|
+
adapter = cls(context)
|
|
33
|
+
check_adapter(adapter)
|
|
34
|
+
return adapter
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
def write_template(name: str, out_dir: str | Path = ".") -> Path:
|
|
38
|
+
"""Write ``<name>_adapter.py`` from TEMPLATE; refuse to overwrite."""
|
|
39
|
+
if not name.isidentifier():
|
|
40
|
+
raise AdapterError(f"{name!r} is not a Python identifier")
|
|
41
|
+
path = Path(out_dir) / f"{name}_adapter.py"
|
|
42
|
+
if path.exists():
|
|
43
|
+
raise AdapterError(f"{path} already exists; not overwriting it")
|
|
44
|
+
path.write_text(TEMPLATE.replace("{NAME}", name), encoding="utf-8")
|
|
45
|
+
return path
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
TEMPLATE = '''"""{NAME}: a Bounded Embodiment adapter (written by `be adapter new {NAME}`).
|
|
49
|
+
|
|
50
|
+
DRAFT — requires line-by-line human review before use on hardware.
|
|
51
|
+
|
|
52
|
+
Fill in the four places marked TODO. Run it with:
|
|
53
|
+
|
|
54
|
+
be run ev03 --mode shakedown --adapter python --adapter-class {NAME}_adapter.py:Adapter ...
|
|
55
|
+
|
|
56
|
+
Rules the runner relies on (bounded_embodiment_kit.adapters.base has the full contract):
|
|
57
|
+
|
|
58
|
+
- open() connects WITHOUT the robot jumping: set every position goal to where the joint already is
|
|
59
|
+
before anything enables torque. It returns the lab's declared command space, loaded from the
|
|
60
|
+
file, never built from what the robot reports.
|
|
61
|
+
- send() never raises. It hands the command to your system under test (your gate, your model's
|
|
62
|
+
command interface) exactly as your model would, and reports what happened.
|
|
63
|
+
- latch_stop() is called from the WATCHDOG thread: it only sets a flag. No bus access, no lock.
|
|
64
|
+
- execute_stop() runs on the command thread, under the bus lock. It chooses the stop goals ONCE
|
|
65
|
+
per latch and re-sends exactly the same goals on every later call. Never re-read the present
|
|
66
|
+
position on a later stop: a sagging arm re-anchored on every stop ratchets downward.
|
|
67
|
+
- close() leaves the robot holding (torque on) unless the operator types OFF.
|
|
68
|
+
"""
|
|
69
|
+
import threading
|
|
70
|
+
|
|
71
|
+
from bounded_embodiment.commandspace import load_command_space
|
|
72
|
+
from bounded_embodiment_kit.adapters.base import SendResult, State
|
|
73
|
+
|
|
74
|
+
|
|
75
|
+
class Adapter:
|
|
76
|
+
name = "{NAME}"
|
|
77
|
+
simulation = False
|
|
78
|
+
|
|
79
|
+
def __init__(self, context):
|
|
80
|
+
self.context = context # AdapterContext: paths, mode, prompt, log
|
|
81
|
+
self._bus_lock = threading.RLock() # every bus access takes it
|
|
82
|
+
self._latched = threading.Event()
|
|
83
|
+
self._stop_goals = None
|
|
84
|
+
self.robot = None
|
|
85
|
+
|
|
86
|
+
def open(self):
|
|
87
|
+
space = load_command_space(self.context.command_space_path)
|
|
88
|
+
# TODO: connect to the robot WITHOUT enabling torque; set every goal to the present
|
|
89
|
+
# position; read the goals back; only then enable torque. Then wrap it in your gate.
|
|
90
|
+
raise NotImplementedError("{NAME}: open() is not written yet")
|
|
91
|
+
self._latched.clear()
|
|
92
|
+
self._stop_goals = None
|
|
93
|
+
return space
|
|
94
|
+
|
|
95
|
+
def describe(self):
|
|
96
|
+
# "config" goes into the setup hash: everything that changes what is tested.
|
|
97
|
+
return {"adapter": self.name, "simulation": False,
|
|
98
|
+
"config": {"adapter": self.name}, # TODO: port, robot id, gate version...
|
|
99
|
+
"joint_speed_trip_per_s": {}} # e.g. {"elbow": 150.0} (deg/s)
|
|
100
|
+
|
|
101
|
+
def send(self, cmd):
|
|
102
|
+
if self._latched.is_set():
|
|
103
|
+
return SendResult(False, None, "stop_latched")
|
|
104
|
+
try:
|
|
105
|
+
with self._bus_lock:
|
|
106
|
+
if self._latched.is_set():
|
|
107
|
+
return SendResult(False, None, "stop_latched")
|
|
108
|
+
# TODO: hand cmd to your system under test, exactly as your model would.
|
|
109
|
+
raise NotImplementedError
|
|
110
|
+
except Exception as e: # send() never raises
|
|
111
|
+
return SendResult(False, None, f"exception:{type(e).__name__}")
|
|
112
|
+
return SendResult(True, None, None)
|
|
113
|
+
|
|
114
|
+
def read_state(self):
|
|
115
|
+
# TODO: return State(joints={name: value}, units={name: "deg"}) from the encoders, or
|
|
116
|
+
# None if the robot has no joint state (then a run needs --no-fk-tripwire-i-understand).
|
|
117
|
+
return None
|
|
118
|
+
|
|
119
|
+
def latch_stop(self):
|
|
120
|
+
self._latched.set() # watchdog thread: a flag, nothing else
|
|
121
|
+
|
|
122
|
+
def execute_stop(self):
|
|
123
|
+
self._latched.set()
|
|
124
|
+
with self._bus_lock:
|
|
125
|
+
if self._stop_goals is None:
|
|
126
|
+
# TODO: choose the goals ONCE: read every joint's present position (a joint may
|
|
127
|
+
# be moving, so stop it where it is). Never read them again for a later stop.
|
|
128
|
+
self._stop_goals = {}
|
|
129
|
+
# TODO: send self._stop_goals to the robot, bypassing the gate.
|
|
130
|
+
|
|
131
|
+
def close(self):
|
|
132
|
+
answer = self.context.prompt("Type OFF and Enter to turn torque off, or Enter to leave it on: ")
|
|
133
|
+
if answer.strip() == "OFF":
|
|
134
|
+
pass # TODO: torque off
|
|
135
|
+
# TODO: release the connection without touching torque.
|
|
136
|
+
'''
|
|
@@ -0,0 +1,206 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Craig Merry and contributors
|
|
2
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
3
|
+
"""DRAFT — requires line-by-line human review before use on hardware.
|
|
4
|
+
|
|
5
|
+
``sim-rover``: a simulated differential-drive rover commanded with ``{"linear": m/s, "angular":
|
|
6
|
+
rad/s}`` (``examples/command-spaces/rover.command-space.json``). Port of ev03-harness
|
|
7
|
+
``ev03/adapters.py`` SimRover, with its constants made parameters.
|
|
8
|
+
|
|
9
|
+
The drive's own command interface (``SimRover.send_action``) does what UngatedRover did in the
|
|
10
|
+
harness: a command with both keys as finite numbers is clamped to the drive's top speeds and
|
|
11
|
+
applied; anything else raises ValueError and changes nothing. A lab's gate can wrap it with
|
|
12
|
+
``--wrapper module:factory`` exactly as for an arm: ``factory(rover, context)``.
|
|
13
|
+
|
|
14
|
+
The stop is zero velocity, the same every time. The rover then brakes at its acceleration limit,
|
|
15
|
+
so from top speed it needs top_speed**2 / (2 * accel) metres (1.1 m with the defaults) and more
|
|
16
|
+
than the watchdog's 0.25 s acknowledgement window: a stop at speed escalates, by design.
|
|
17
|
+
|
|
18
|
+
There are no kinematics to check from the drive's state, so a run on it has no FK tripwire.
|
|
19
|
+
"""
|
|
20
|
+
from __future__ import annotations
|
|
21
|
+
|
|
22
|
+
import math
|
|
23
|
+
import threading
|
|
24
|
+
import time
|
|
25
|
+
from collections.abc import Callable, Mapping
|
|
26
|
+
from pathlib import Path
|
|
27
|
+
|
|
28
|
+
from bounded_embodiment.commandspace import CommandSpace, load_command_space
|
|
29
|
+
|
|
30
|
+
from .base import AdapterError, SendResult, State, clean_text, load_callable
|
|
31
|
+
|
|
32
|
+
__all__ = ["SimRover", "SimRoverAdapter", "add_arguments", "from_args"]
|
|
33
|
+
|
|
34
|
+
PHYSICS_PERIOD_S = 0.005
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
def _number(value: object) -> float:
|
|
38
|
+
if isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value):
|
|
39
|
+
raise ValueError(f"not a finite number: {value!r}")
|
|
40
|
+
return float(value)
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
class SimRover:
|
|
44
|
+
"""Unicycle rover. Ground truth is (x, y, theta) in the envelope frame, metres and radians."""
|
|
45
|
+
|
|
46
|
+
def __init__(self, top_speed: float = 1.5, max_turn: float = 1.5, accel: float = 1.0,
|
|
47
|
+
start: tuple[float, float, float] = (1.0, 1.0, 0.0)):
|
|
48
|
+
self.x, self.y, self.th = (float(v) for v in start)
|
|
49
|
+
self.v = self.w = 0.0
|
|
50
|
+
self.cmd_v = self.cmd_w = 0.0
|
|
51
|
+
self.top, self.max_turn, self.accel = top_speed, max_turn, accel
|
|
52
|
+
self.lock = threading.Lock()
|
|
53
|
+
|
|
54
|
+
def actuate(self, linear_mps: float, angular_radps: float) -> None:
|
|
55
|
+
"""What reaches the motors, clamped to what the drive can do."""
|
|
56
|
+
with self.lock:
|
|
57
|
+
self.cmd_v = max(-self.top, min(self.top, linear_mps))
|
|
58
|
+
self.cmd_w = max(-self.max_turn, min(self.max_turn, angular_radps))
|
|
59
|
+
|
|
60
|
+
def send_action(self, cmd: Mapping) -> dict:
|
|
61
|
+
"""The drive's own command interface: both keys, finite numbers, or ValueError."""
|
|
62
|
+
if not isinstance(cmd, Mapping) or "linear" not in cmd or "angular" not in cmd:
|
|
63
|
+
raise ValueError(f"a drive command needs linear and angular: {cmd!r}")
|
|
64
|
+
v, w = _number(cmd["linear"]), _number(cmd["angular"])
|
|
65
|
+
self.actuate(v, w)
|
|
66
|
+
return {"linear": self.cmd_v, "angular": self.cmd_w}
|
|
67
|
+
|
|
68
|
+
def tick(self, dt: float) -> None:
|
|
69
|
+
with self.lock:
|
|
70
|
+
dv = max(-self.accel * dt, min(self.accel * dt, self.cmd_v - self.v))
|
|
71
|
+
self.v += dv
|
|
72
|
+
self.w = self.cmd_w
|
|
73
|
+
self.th += self.w * dt
|
|
74
|
+
self.x += self.v * math.cos(self.th) * dt
|
|
75
|
+
self.y += self.v * math.sin(self.th) * dt
|
|
76
|
+
|
|
77
|
+
def pose(self) -> tuple[float, float, float]:
|
|
78
|
+
with self.lock:
|
|
79
|
+
return self.x, self.y, self.th
|
|
80
|
+
|
|
81
|
+
def velocities(self) -> tuple[float, float]:
|
|
82
|
+
with self.lock:
|
|
83
|
+
return self.v, self.w
|
|
84
|
+
|
|
85
|
+
|
|
86
|
+
class SimRoverAdapter:
|
|
87
|
+
"""The adapter contract for the simulated rover."""
|
|
88
|
+
|
|
89
|
+
name = "sim-rover"
|
|
90
|
+
simulation = True
|
|
91
|
+
|
|
92
|
+
def __init__(self, *, command_space_path: str | Path, top_speed: float = 1.5,
|
|
93
|
+
max_turn: float = 1.5, accel: float = 1.0,
|
|
94
|
+
start: tuple[float, float, float] = (1.0, 1.0, 0.0), wrapper: str | None = None,
|
|
95
|
+
wrapper_context: dict | None = None, realtime: bool = True,
|
|
96
|
+
log: Callable[[str], None] = print, prompt: Callable[[str], str] = input):
|
|
97
|
+
self.command_space_path = Path(command_space_path)
|
|
98
|
+
self.params = {"top_speed": top_speed, "max_turn": max_turn, "accel": accel,
|
|
99
|
+
"start": list(start)}
|
|
100
|
+
self.wrapper, self.wrapper_context = wrapper, dict(wrapper_context or {})
|
|
101
|
+
self.realtime, self.log, self.prompt = realtime, log, prompt
|
|
102
|
+
self._bus_lock = threading.RLock() # the drive's "bus": one lock, as for an arm
|
|
103
|
+
self._latched = threading.Event()
|
|
104
|
+
self._stop_goals: dict[str, float] | None = None
|
|
105
|
+
self.rover: SimRover | None = None
|
|
106
|
+
self._target = None
|
|
107
|
+
self._physics: threading.Thread | None = None
|
|
108
|
+
self._physics_quit = threading.Event()
|
|
109
|
+
|
|
110
|
+
def open(self) -> CommandSpace:
|
|
111
|
+
space = load_command_space(self.command_space_path)
|
|
112
|
+
if space.kind != "velocity" or set(space.keys) != {"linear", "angular"}:
|
|
113
|
+
raise AdapterError("sim-rover takes a velocity command space with axes linear and angular")
|
|
114
|
+
factory = load_callable(self.wrapper) if self.wrapper else None
|
|
115
|
+
p = self.params
|
|
116
|
+
self.rover = SimRover(p["top_speed"], p["max_turn"], p["accel"], tuple(p["start"]))
|
|
117
|
+
self._target = factory(self.rover, dict(self.wrapper_context)) if factory else self.rover
|
|
118
|
+
if not callable(getattr(self._target, "send_action", None)):
|
|
119
|
+
raise AdapterError(f"{self.wrapper} returned an object without send_action")
|
|
120
|
+
self._stop_goals = None
|
|
121
|
+
self._latched.clear()
|
|
122
|
+
if self.realtime:
|
|
123
|
+
self._physics_quit.clear()
|
|
124
|
+
self._physics = threading.Thread(target=self._run_physics, name="be-sim-physics",
|
|
125
|
+
daemon=True)
|
|
126
|
+
self._physics.start()
|
|
127
|
+
return space
|
|
128
|
+
|
|
129
|
+
def _run_physics(self) -> None:
|
|
130
|
+
last = time.perf_counter()
|
|
131
|
+
while not self._physics_quit.wait(PHYSICS_PERIOD_S):
|
|
132
|
+
now = time.perf_counter()
|
|
133
|
+
self.rover.tick(now - last)
|
|
134
|
+
last = now
|
|
135
|
+
|
|
136
|
+
def describe(self) -> dict:
|
|
137
|
+
return {"adapter": self.name, "simulation": True,
|
|
138
|
+
"config": {"adapter": self.name, "wrapper": self.wrapper, "sim": dict(self.params)},
|
|
139
|
+
"stop": {"rule": "zero velocity; the rover brakes at its acceleration limit"},
|
|
140
|
+
"joint_speed_trip_per_s": {}}
|
|
141
|
+
|
|
142
|
+
def send(self, cmd: dict) -> SendResult:
|
|
143
|
+
if self._latched.is_set():
|
|
144
|
+
return SendResult(False, None, "stop_latched")
|
|
145
|
+
try:
|
|
146
|
+
with self._bus_lock:
|
|
147
|
+
if self._latched.is_set():
|
|
148
|
+
return SendResult(False, None, "stop_latched")
|
|
149
|
+
self._target.send_action(cmd)
|
|
150
|
+
except Exception as e: # noqa: BLE001 - send() never raises; the error is the result
|
|
151
|
+
return SendResult(False, None, f"exception:{type(e).__name__}")
|
|
152
|
+
decision = getattr(self._target, "last_decision", None) if self._target is not self.rover else None
|
|
153
|
+
return SendResult(True, clean_text(decision), None)
|
|
154
|
+
|
|
155
|
+
def read_state(self) -> State | None:
|
|
156
|
+
with self._bus_lock:
|
|
157
|
+
v, w = self.rover.velocities()
|
|
158
|
+
return State(joints={"linear": v, "angular": w}, units={"linear": "m/s", "angular": "rad/s"})
|
|
159
|
+
|
|
160
|
+
def latch_stop(self) -> None:
|
|
161
|
+
self._latched.set()
|
|
162
|
+
|
|
163
|
+
def execute_stop(self) -> None:
|
|
164
|
+
self._latched.set()
|
|
165
|
+
with self._bus_lock:
|
|
166
|
+
if self._stop_goals is None:
|
|
167
|
+
self._stop_goals = {"linear": 0.0, "angular": 0.0}
|
|
168
|
+
self.rover.actuate(self._stop_goals["linear"], self._stop_goals["angular"])
|
|
169
|
+
|
|
170
|
+
def close(self) -> None:
|
|
171
|
+
self._physics_quit.set()
|
|
172
|
+
if self._physics is not None:
|
|
173
|
+
self._physics.join(timeout=2.0)
|
|
174
|
+
self._physics = None
|
|
175
|
+
|
|
176
|
+
def step(self, dt: float, substep: float = 0.001) -> None:
|
|
177
|
+
n = max(1, round(dt / substep))
|
|
178
|
+
for _ in range(n):
|
|
179
|
+
self.rover.tick(dt / n)
|
|
180
|
+
|
|
181
|
+
def truth_fn(self, fk=None, envelope=None) -> Callable[[], dict]:
|
|
182
|
+
"""For the simulated instrument: every envelope point at the rover's position, z = 0."""
|
|
183
|
+
points = list(getattr(envelope, "points", {}) or {"base": None})
|
|
184
|
+
|
|
185
|
+
def truth() -> dict:
|
|
186
|
+
x, y, _ = self.rover.pose()
|
|
187
|
+
return {pid: (x, y, 0.0) for pid in points}
|
|
188
|
+
|
|
189
|
+
return truth
|
|
190
|
+
|
|
191
|
+
|
|
192
|
+
def add_arguments(p) -> None:
|
|
193
|
+
g = p.add_argument_group("sim-rover adapter")
|
|
194
|
+
g.add_argument("--sim-rover-start", default="1.0,1.0,0.0", metavar="X,Y,THETA",
|
|
195
|
+
help="start pose in the envelope frame (m, m, rad)")
|
|
196
|
+
|
|
197
|
+
|
|
198
|
+
def from_args(args, context) -> SimRoverAdapter:
|
|
199
|
+
try:
|
|
200
|
+
x, y, th = (float(v) for v in args.sim_rover_start.split(","))
|
|
201
|
+
except ValueError:
|
|
202
|
+
raise AdapterError(f"--sim-rover-start {args.sim_rover_start!r} is not X,Y,THETA") from None
|
|
203
|
+
return SimRoverAdapter(command_space_path=context.command_space_path, start=(x, y, th),
|
|
204
|
+
wrapper=getattr(args, "wrapper", None),
|
|
205
|
+
wrapper_context=context.wrapper_context(), log=context.log,
|
|
206
|
+
prompt=context.prompt)
|