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.
Files changed (49) hide show
  1. bounded_embodiment_kit/__init__.py +2 -0
  2. bounded_embodiment_kit/adapters/__init__.py +29 -0
  3. bounded_embodiment_kit/adapters/base.py +192 -0
  4. bounded_embodiment_kit/adapters/feetech.py +204 -0
  5. bounded_embodiment_kit/adapters/lerobot.py +316 -0
  6. bounded_embodiment_kit/adapters/python.py +136 -0
  7. bounded_embodiment_kit/adapters/sim_rover.py +206 -0
  8. bounded_embodiment_kit/adapters/sim_so101.py +615 -0
  9. bounded_embodiment_kit/cli.py +23 -0
  10. bounded_embodiment_kit/instruments/__init__.py +1 -0
  11. bounded_embodiment_kit/instruments/accuracy.py +866 -0
  12. bounded_embodiment_kit/instruments/markerless/__init__.py +16 -0
  13. bounded_embodiment_kit/instruments/markerless/background.py +123 -0
  14. bounded_embodiment_kit/instruments/markerless/cli.py +319 -0
  15. bounded_embodiment_kit/instruments/markerless/fit.py +1346 -0
  16. bounded_embodiment_kit/instruments/markerless/mesh.py +253 -0
  17. bounded_embodiment_kit/instruments/markerless/model.py +316 -0
  18. bounded_embodiment_kit/instruments/markerless/register.py +368 -0
  19. bounded_embodiment_kit/instruments/markerless/synth.py +669 -0
  20. bounded_embodiment_kit/instruments/plan.py +279 -0
  21. bounded_embodiment_kit/instruments/record.py +2481 -0
  22. bounded_embodiment_kit/instruments/session.py +291 -0
  23. bounded_embodiment_kit/kinematics/__init__.py +5 -0
  24. bounded_embodiment_kit/kinematics/ik.py +109 -0
  25. bounded_embodiment_kit/kinematics/urdf.py +401 -0
  26. bounded_embodiment_kit/runner/__init__.py +4 -0
  27. bounded_embodiment_kit/runner/cli.py +170 -0
  28. bounded_embodiment_kit/runner/doctor.py +128 -0
  29. bounded_embodiment_kit/runner/feeds.py +315 -0
  30. bounded_embodiment_kit/runner/interlocks.py +122 -0
  31. bounded_embodiment_kit/runner/kin.py +75 -0
  32. bounded_embodiment_kit/runner/limits.py +116 -0
  33. bounded_embodiment_kit/runner/run.py +701 -0
  34. bounded_embodiment_kit/runner/session.py +422 -0
  35. bounded_embodiment_kit/runner/watchdog.py +484 -0
  36. bounded_embodiment_kit/submission/__init__.py +11 -0
  37. bounded_embodiment_kit/submission/cli.py +96 -0
  38. bounded_embodiment_kit/submission/files.py +127 -0
  39. bounded_embodiment_kit/submission/ids.py +91 -0
  40. bounded_embodiment_kit/submission/lab-config-0.1.json +50 -0
  41. bounded_embodiment_kit/submission/labconfig.py +77 -0
  42. bounded_embodiment_kit/submission/report.py +733 -0
  43. bounded_embodiment_kit/submission/scrub.py +342 -0
  44. bounded_embodiment_kit/submission/submit.py +328 -0
  45. bounded_embodiment_kit/submission/timebase.py +396 -0
  46. bounded_embodiment_kit-0.1.0.dist-info/METADATA +39 -0
  47. bounded_embodiment_kit-0.1.0.dist-info/RECORD +49 -0
  48. bounded_embodiment_kit-0.1.0.dist-info/WHEEL +4 -0
  49. 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)