sim2bot 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.
- sim2bot/__init__.py +38 -0
- sim2bot/bridge.py +232 -0
- sim2bot/bridge_server.py +488 -0
- sim2bot/cli.py +292 -0
- sim2bot/client.py +1543 -0
- sim2bot-0.1.0.dist-info/METADATA +226 -0
- sim2bot-0.1.0.dist-info/RECORD +11 -0
- sim2bot-0.1.0.dist-info/WHEEL +5 -0
- sim2bot-0.1.0.dist-info/entry_points.txt +2 -0
- sim2bot-0.1.0.dist-info/licenses/LICENSE +21 -0
- sim2bot-0.1.0.dist-info/top_level.txt +1 -0
sim2bot/client.py
ADDED
|
@@ -0,0 +1,1543 @@
|
|
|
1
|
+
"""Sim2Bot Python client.
|
|
2
|
+
|
|
3
|
+
A blocking, thread-based client over the local bridge. The control plane (commands
|
|
4
|
+
+ telemetry + discovery) runs on the JSON WebSocket; camera feeds run on a
|
|
5
|
+
separate binary WebSocket, mirroring the simulator's two-stream design.
|
|
6
|
+
|
|
7
|
+
Design notes:
|
|
8
|
+
- "Always read the present": both telemetry and camera frames keep only the
|
|
9
|
+
latest value; slow consumers drop intermediate frames instead of lagging.
|
|
10
|
+
- Camera subscriptions are renewed on a timer (the simulator expires a feed a
|
|
11
|
+
few seconds after the last renewal), so a crashed consumer stops the feed.
|
|
12
|
+
"""
|
|
13
|
+
|
|
14
|
+
from __future__ import annotations
|
|
15
|
+
|
|
16
|
+
import json
|
|
17
|
+
import os
|
|
18
|
+
import socket
|
|
19
|
+
import struct
|
|
20
|
+
import threading
|
|
21
|
+
import time
|
|
22
|
+
from dataclasses import dataclass, field
|
|
23
|
+
from typing import Any, Callable, Iterable, Iterator, Optional, Sequence, Union
|
|
24
|
+
from urllib.parse import urlparse, urlunparse
|
|
25
|
+
|
|
26
|
+
from websockets.exceptions import ConnectionClosed
|
|
27
|
+
from websockets.sync.client import connect as ws_connect
|
|
28
|
+
|
|
29
|
+
# Frame header: must match src/bridge/videoFrame.ts.
|
|
30
|
+
# u8 version, u8 codec, u16 flags, u32 sequence, f64 captureTs, u16 width,
|
|
31
|
+
# u16 height, then u8 cameraIdLen, cameraId (utf-8), frame bytes.
|
|
32
|
+
_HEADER = struct.Struct("<BBHIdHH") # 20 bytes
|
|
33
|
+
_CODEC_JPEG = 0
|
|
34
|
+
_CODEC_H264 = 1
|
|
35
|
+
_CODEC_RAW = 2
|
|
36
|
+
_FLAG_KEYFRAME = 1
|
|
37
|
+
_CODEC_BY_NAME = {"jpeg": _CODEC_JPEG, "h264": _CODEC_H264, "raw": _CODEC_RAW}
|
|
38
|
+
|
|
39
|
+
# Re-send camera_subscribe at this interval to keep the feed alive (the sim's TTL
|
|
40
|
+
# is a few seconds — see CameraStreamManager).
|
|
41
|
+
_RENEW_INTERVAL_S = 1.0
|
|
42
|
+
|
|
43
|
+
|
|
44
|
+
@dataclass
|
|
45
|
+
class RobotInfo:
|
|
46
|
+
"""Model-derived metadata for one robot announced by the simulator.
|
|
47
|
+
|
|
48
|
+
Attributes:
|
|
49
|
+
index: Current scene command address. Pass it as ``robot=`` to commands.
|
|
50
|
+
id: Stable identity while this scene instance is loaded.
|
|
51
|
+
name: Human-readable model or instance name.
|
|
52
|
+
dof: Number of controllable arm joints, excluding gripper joints.
|
|
53
|
+
joint_names: Joint names in the exact order expected by commands.
|
|
54
|
+
joint_limits: Per-joint ``[lower, upper]`` limits in radians, or ``None``
|
|
55
|
+
for an unlimited joint.
|
|
56
|
+
home: Model-defined home target in radians and command order.
|
|
57
|
+
has_gripper: Whether the robot exposes a supported gripper.
|
|
58
|
+
locomotion: Capability label such as ``"arm"``, mobile, or aerial.
|
|
59
|
+
base_dof: Number of supported mobile or aerial base degrees of freedom.
|
|
60
|
+
category: Sim2Bot catalog category.
|
|
61
|
+
manufacturer: Authored manufacturer, when declared.
|
|
62
|
+
license: SPDX licence identifier, when declared.
|
|
63
|
+
source_url: Upstream model source, when declared.
|
|
64
|
+
snapshot_digest: SHA-256 identity of the exact bundled model snapshot.
|
|
65
|
+
capabilities: Verified Sim2Bot capability labels.
|
|
66
|
+
arm_actuators: Authored arm actuator names in command order.
|
|
67
|
+
gripper_actuators: Authored gripper actuator names.
|
|
68
|
+
tcp_sites: Authored tool-centre-point site candidates.
|
|
69
|
+
end_effector_bodies: Authored end-effector body candidates.
|
|
70
|
+
keyframes: Named MJCF poses such as ``home`` or ``rest``.
|
|
71
|
+
model_cameras: Cameras compiled as part of this robot model.
|
|
72
|
+
model_sensors: Sensors compiled as part of this robot model.
|
|
73
|
+
"""
|
|
74
|
+
|
|
75
|
+
index: int
|
|
76
|
+
"""Current scene command address passed as ``robot=`` to control methods."""
|
|
77
|
+
id: str
|
|
78
|
+
"""Stable robot identity while the current scene instance remains loaded."""
|
|
79
|
+
name: str
|
|
80
|
+
"""Human-readable robot model or scene-instance name."""
|
|
81
|
+
dof: int
|
|
82
|
+
"""Number of controllable arm joints, excluding gripper joints."""
|
|
83
|
+
joint_names: list[str] = field(default_factory=list)
|
|
84
|
+
"""Joint names in the exact order expected by commands and telemetry."""
|
|
85
|
+
joint_limits: list[Optional[list[float]]] = field(default_factory=list)
|
|
86
|
+
"""Per-joint ``[lower, upper]`` limits in radians, or ``None`` if unlimited."""
|
|
87
|
+
home: list[float] = field(default_factory=list)
|
|
88
|
+
"""Model-defined home joint target in radians and command order."""
|
|
89
|
+
has_gripper: bool = False
|
|
90
|
+
"""Whether the robot exposes a gripper supported by `Robot.gripper`."""
|
|
91
|
+
locomotion: str = "arm"
|
|
92
|
+
"""Capability label such as ``"arm"``, ``"mobile"``, or ``"aerial"``."""
|
|
93
|
+
base_dof: int = 0
|
|
94
|
+
"""Number of supported mobile or aerial base degrees of freedom."""
|
|
95
|
+
category: str = "custom"
|
|
96
|
+
"""Sim2Bot catalog category such as ``arm`` or ``mobile-base``."""
|
|
97
|
+
manufacturer: str = ""
|
|
98
|
+
"""Authored robot manufacturer, if declared by the model package."""
|
|
99
|
+
license: str = ""
|
|
100
|
+
"""SPDX licence identifier for the model package, if declared."""
|
|
101
|
+
source_url: str = ""
|
|
102
|
+
"""Upstream source URL for the model package, if declared."""
|
|
103
|
+
source_revision: str = ""
|
|
104
|
+
"""Pinned upstream source revision, if declared."""
|
|
105
|
+
snapshot_digest: str = ""
|
|
106
|
+
"""SHA-256 identity of the exact bundled model snapshot, if available."""
|
|
107
|
+
capabilities: list[str] = field(default_factory=list)
|
|
108
|
+
"""Verified Sim2Bot capability labels for this model."""
|
|
109
|
+
arm_actuators: list[str] = field(default_factory=list)
|
|
110
|
+
"""Authored arm actuator names in command order."""
|
|
111
|
+
gripper_actuators: list[str] = field(default_factory=list)
|
|
112
|
+
"""Authored gripper actuator names."""
|
|
113
|
+
tcp_sites: list[str] = field(default_factory=list)
|
|
114
|
+
"""Authored TCP site candidates in preference order."""
|
|
115
|
+
end_effector_bodies: list[str] = field(default_factory=list)
|
|
116
|
+
"""Authored end-effector body candidates in preference order."""
|
|
117
|
+
keyframes: list[str] = field(default_factory=list)
|
|
118
|
+
"""Named MJCF keyframe poses exposed by the robot."""
|
|
119
|
+
model_cameras: list[str] = field(default_factory=list)
|
|
120
|
+
"""Camera names compiled as part of this robot model."""
|
|
121
|
+
model_sensors: list[str] = field(default_factory=list)
|
|
122
|
+
"""Sensor names compiled as part of this robot model."""
|
|
123
|
+
variant: str = ""
|
|
124
|
+
"""Selected end-effector/model variant label, if applicable."""
|
|
125
|
+
|
|
126
|
+
@classmethod
|
|
127
|
+
def from_json(cls, data: dict) -> "RobotInfo":
|
|
128
|
+
return cls(
|
|
129
|
+
index=int(data.get("index", 0)),
|
|
130
|
+
id=str(data.get("id") or ""),
|
|
131
|
+
name=str(data.get("name", "")),
|
|
132
|
+
dof=int(data.get("dof", 0)),
|
|
133
|
+
joint_names=list(data.get("jointNames", []) or []),
|
|
134
|
+
joint_limits=list(data.get("jointLimits", []) or []),
|
|
135
|
+
home=list(data.get("home", []) or []),
|
|
136
|
+
has_gripper=bool(data.get("hasGripper", False)),
|
|
137
|
+
locomotion=str(data.get("locomotion", "arm")),
|
|
138
|
+
base_dof=int(data.get("baseDof", 0)),
|
|
139
|
+
category=str(data.get("category") or "custom"),
|
|
140
|
+
manufacturer=str(data.get("manufacturer") or ""),
|
|
141
|
+
license=str(data.get("license") or ""),
|
|
142
|
+
source_url=str(data.get("sourceUrl") or ""),
|
|
143
|
+
source_revision=str(data.get("sourceRevision") or ""),
|
|
144
|
+
snapshot_digest=str(data.get("snapshotDigest") or ""),
|
|
145
|
+
capabilities=list(data.get("capabilities", []) or []),
|
|
146
|
+
arm_actuators=list(data.get("armActuators", []) or []),
|
|
147
|
+
gripper_actuators=list(data.get("gripperActuators", []) or []),
|
|
148
|
+
tcp_sites=list(data.get("tcpSites", []) or []),
|
|
149
|
+
end_effector_bodies=list(data.get("endEffectorBodies", []) or []),
|
|
150
|
+
keyframes=list(data.get("keyframes", []) or []),
|
|
151
|
+
model_cameras=list(data.get("modelCameras", []) or []),
|
|
152
|
+
model_sensors=list(data.get("modelSensors", []) or []),
|
|
153
|
+
variant=str(data.get("variant") or ""),
|
|
154
|
+
)
|
|
155
|
+
|
|
156
|
+
|
|
157
|
+
@dataclass
|
|
158
|
+
class RoomDeviceInfo:
|
|
159
|
+
"""Metadata for a door or window announced by the current scene.
|
|
160
|
+
|
|
161
|
+
Room-device control is experimental while browser-side actuation is being
|
|
162
|
+
revised. Discovery is safe to use, but do not build critical workflows around
|
|
163
|
+
opening behavior yet.
|
|
164
|
+
|
|
165
|
+
Attributes:
|
|
166
|
+
id: Scene-unique device identity used by ``set_room_opening()``.
|
|
167
|
+
name: Human-readable device name.
|
|
168
|
+
kind: Authored type, normally ``"door"`` or ``"window"``.
|
|
169
|
+
motion: Mechanism type, for example hinged, sliding, or fixed.
|
|
170
|
+
wall: ID of the wall that owns the opening.
|
|
171
|
+
max_open_deg: Maximum authored hinge travel in degrees when applicable.
|
|
172
|
+
"""
|
|
173
|
+
|
|
174
|
+
id: str
|
|
175
|
+
name: str
|
|
176
|
+
kind: str
|
|
177
|
+
motion: str
|
|
178
|
+
wall: str
|
|
179
|
+
max_open_deg: float
|
|
180
|
+
|
|
181
|
+
@classmethod
|
|
182
|
+
def from_json(cls, data: dict) -> "RoomDeviceInfo":
|
|
183
|
+
return cls(
|
|
184
|
+
id=str(data.get("id") or ""),
|
|
185
|
+
name=str(data.get("name") or ""),
|
|
186
|
+
kind=str(data.get("kind") or ""),
|
|
187
|
+
motion=str(data.get("motion") or "fixed"),
|
|
188
|
+
wall=str(data.get("wall") or ""),
|
|
189
|
+
max_open_deg=float(data.get("maxOpenDeg", 0.0)),
|
|
190
|
+
)
|
|
191
|
+
|
|
192
|
+
|
|
193
|
+
@dataclass
|
|
194
|
+
class RobotState:
|
|
195
|
+
"""Latest complete telemetry packet for one robot.
|
|
196
|
+
|
|
197
|
+
Joint quantities use the command order announced by
|
|
198
|
+
[`RobotInfo`][sim2bot.client.RobotInfo].
|
|
199
|
+
TCP quantities are expressed in the world frame. Derivatives are unsmoothed;
|
|
200
|
+
any smoothing selected in the GUI is display-only.
|
|
201
|
+
|
|
202
|
+
Attributes:
|
|
203
|
+
robot: Current scene robot index.
|
|
204
|
+
id: Robot scene identity.
|
|
205
|
+
name: Human-readable robot name.
|
|
206
|
+
dof: Number of reported arm joints.
|
|
207
|
+
t: MuJoCo simulation time in seconds.
|
|
208
|
+
q: Joint positions in radians.
|
|
209
|
+
target: Latest joint-position targets in radians.
|
|
210
|
+
qd: Joint velocities in radians per second.
|
|
211
|
+
qdd: Joint accelerations in radians per second squared.
|
|
212
|
+
qddd: Joint jerk in radians per second cubed.
|
|
213
|
+
tcp: TCP world position ``[x, y, z]`` in metres.
|
|
214
|
+
tcp_orientation: TCP world quaternion ``[x, y, z, w]``.
|
|
215
|
+
tcp_linear_velocity: World-frame linear velocity in metres per second.
|
|
216
|
+
tcp_angular_velocity: World-frame angular velocity in radians per second.
|
|
217
|
+
tcp_linear_acceleration: Linear acceleration in metres per second squared.
|
|
218
|
+
tcp_angular_acceleration: Angular acceleration in radians per second squared.
|
|
219
|
+
tcp_linear_jerk: Linear jerk in metres per second cubed.
|
|
220
|
+
tcp_angular_jerk: Angular jerk in radians per second cubed.
|
|
221
|
+
gripper: Commanded opening fraction from 0 closed to 1 open, if present.
|
|
222
|
+
sensors: Authored sensor readings associated with this robot packet.
|
|
223
|
+
room_devices: Scene door/window states, currently carried by robot 0.
|
|
224
|
+
capture_ts: Unix capture timestamp for the newest packet sample.
|
|
225
|
+
samples: Physics-substep ``{t, q, qd}`` samples since the prior packet.
|
|
226
|
+
"""
|
|
227
|
+
|
|
228
|
+
robot: int
|
|
229
|
+
"""Current scene robot index used by addressed SDK commands."""
|
|
230
|
+
id: str
|
|
231
|
+
"""Stable robot identity while the current scene instance remains loaded."""
|
|
232
|
+
name: str
|
|
233
|
+
"""Human-readable robot model or scene-instance name."""
|
|
234
|
+
dof: int
|
|
235
|
+
"""Number of reported controllable arm joints."""
|
|
236
|
+
t: float
|
|
237
|
+
"""MuJoCo simulation time in seconds for the newest sample."""
|
|
238
|
+
q: list[float]
|
|
239
|
+
"""Joint positions in radians and discovered joint order."""
|
|
240
|
+
target: list[float]
|
|
241
|
+
"""Latest joint-position targets in radians and discovered joint order."""
|
|
242
|
+
qd: list[float] = field(default_factory=list)
|
|
243
|
+
"""Joint velocities in radians per second."""
|
|
244
|
+
qdd: list[float] = field(default_factory=list)
|
|
245
|
+
"""Unsmoothed joint accelerations in radians per second squared."""
|
|
246
|
+
qddd: list[float] = field(default_factory=list)
|
|
247
|
+
"""Unsmoothed joint jerk in radians per second cubed."""
|
|
248
|
+
tcp: list[float] = field(default_factory=list)
|
|
249
|
+
"""TCP world position ``[x, y, z]`` in metres."""
|
|
250
|
+
tcp_orientation: list[float] = field(default_factory=list)
|
|
251
|
+
"""TCP world orientation quaternion in ``[x, y, z, w]`` order."""
|
|
252
|
+
tcp_linear_velocity: list[float] = field(default_factory=list)
|
|
253
|
+
"""TCP world-frame linear velocity ``[x, y, z]`` in metres per second."""
|
|
254
|
+
tcp_angular_velocity: list[float] = field(default_factory=list)
|
|
255
|
+
"""TCP world-frame angular velocity ``[x, y, z]`` in radians per second."""
|
|
256
|
+
tcp_linear_acceleration: list[float] = field(default_factory=list)
|
|
257
|
+
"""TCP linear acceleration in metres per second squared."""
|
|
258
|
+
tcp_angular_acceleration: list[float] = field(default_factory=list)
|
|
259
|
+
"""TCP angular acceleration in radians per second squared."""
|
|
260
|
+
tcp_linear_jerk: list[float] = field(default_factory=list)
|
|
261
|
+
"""TCP linear jerk in metres per second cubed."""
|
|
262
|
+
tcp_angular_jerk: list[float] = field(default_factory=list)
|
|
263
|
+
"""TCP angular jerk in radians per second cubed."""
|
|
264
|
+
gripper: Optional[float] = None
|
|
265
|
+
"""Commanded opening fraction from 0 closed to 1 open, or ``None``."""
|
|
266
|
+
sensors: list[dict] = field(default_factory=list)
|
|
267
|
+
"""Authored sensor readings associated with this robot telemetry packet."""
|
|
268
|
+
# Scene-level door/window joint states. Present on robot 0 telemetry.
|
|
269
|
+
room_devices: list[dict] = field(default_factory=list)
|
|
270
|
+
"""Scene door/window states, currently carried on robot 0 telemetry."""
|
|
271
|
+
# Wall-clock capture time (unix s) of the newest sample, for latency.
|
|
272
|
+
capture_ts: Optional[float] = None
|
|
273
|
+
"""Browser wall-clock capture timestamp in Unix seconds, if available."""
|
|
274
|
+
# High-rate batch since the previous packet: [{t, q, qd}, ...] at physics rate.
|
|
275
|
+
samples: list[dict] = field(default_factory=list)
|
|
276
|
+
"""Physics-substep ``{t, q, qd}`` samples since the previous packet."""
|
|
277
|
+
|
|
278
|
+
@classmethod
|
|
279
|
+
def from_json(cls, data: dict) -> "RobotState":
|
|
280
|
+
return cls(
|
|
281
|
+
robot=int(data.get("robot", 0)),
|
|
282
|
+
id=str(data.get("id") or ""),
|
|
283
|
+
name=str(data.get("name", "")),
|
|
284
|
+
dof=int(data.get("dof", 0)),
|
|
285
|
+
t=float(data.get("t", 0.0)),
|
|
286
|
+
q=list(data.get("q", []) or []),
|
|
287
|
+
target=list(data.get("target", []) or []),
|
|
288
|
+
qd=list(data.get("qd", []) or []),
|
|
289
|
+
qdd=list(data.get("qdd", []) or []),
|
|
290
|
+
qddd=list(data.get("qddd", []) or []),
|
|
291
|
+
tcp=list(data.get("tcp", []) or []),
|
|
292
|
+
tcp_orientation=list(data.get("tcpOrientation", []) or []),
|
|
293
|
+
tcp_linear_velocity=list(data.get("tcpLinearVelocity", []) or []),
|
|
294
|
+
tcp_angular_velocity=list(data.get("tcpAngularVelocity", []) or []),
|
|
295
|
+
tcp_linear_acceleration=list(data.get("tcpLinearAcceleration", []) or []),
|
|
296
|
+
tcp_angular_acceleration=list(data.get("tcpAngularAcceleration", []) or []),
|
|
297
|
+
tcp_linear_jerk=list(data.get("tcpLinearJerk", []) or []),
|
|
298
|
+
tcp_angular_jerk=list(data.get("tcpAngularJerk", []) or []),
|
|
299
|
+
gripper=data.get("gripper"),
|
|
300
|
+
sensors=list(data.get("sensors", []) or []),
|
|
301
|
+
room_devices=list(data.get("roomDevices", []) or []),
|
|
302
|
+
capture_ts=data.get("captureTs"),
|
|
303
|
+
samples=list(data.get("samples", []) or []),
|
|
304
|
+
)
|
|
305
|
+
|
|
306
|
+
@property
|
|
307
|
+
def latency(self) -> Optional[float]:
|
|
308
|
+
"""Return the current telemetry age in seconds.
|
|
309
|
+
|
|
310
|
+
Returns:
|
|
311
|
+
Seconds since browser capture, or ``None`` when the packet did not
|
|
312
|
+
include a capture timestamp.
|
|
313
|
+
"""
|
|
314
|
+
return (time.time() - self.capture_ts) if self.capture_ts else None
|
|
315
|
+
|
|
316
|
+
|
|
317
|
+
@dataclass
|
|
318
|
+
class CameraFrame:
|
|
319
|
+
"""One encoded camera frame plus capture and stream metadata.
|
|
320
|
+
|
|
321
|
+
Attributes:
|
|
322
|
+
camera_id: Global scene camera ID used for the subscription.
|
|
323
|
+
codec: Numeric wire codec: 0 JPEG, 1 reserved H.264, or 2 raw RGBA.
|
|
324
|
+
keyframe: Whether the frame is marked as independently decodable.
|
|
325
|
+
sequence: Monotonically increasing sequence number for this feed.
|
|
326
|
+
capture_ts: Browser wall-clock capture timestamp in Unix seconds.
|
|
327
|
+
width: Frame width in pixels.
|
|
328
|
+
height: Frame height in pixels.
|
|
329
|
+
data: Encoded image bytes, excluding the Sim2Bot frame header.
|
|
330
|
+
"""
|
|
331
|
+
|
|
332
|
+
camera_id: str
|
|
333
|
+
codec: int
|
|
334
|
+
keyframe: bool
|
|
335
|
+
sequence: int
|
|
336
|
+
capture_ts: float
|
|
337
|
+
width: int
|
|
338
|
+
height: int
|
|
339
|
+
data: bytes
|
|
340
|
+
|
|
341
|
+
@property
|
|
342
|
+
def age(self) -> float:
|
|
343
|
+
"""Return the current frame age in seconds.
|
|
344
|
+
|
|
345
|
+
This is an approximate capture-to-consumer measurement based on wall
|
|
346
|
+
clocks. It includes browser production, bridge relay, and Python delay.
|
|
347
|
+
|
|
348
|
+
Returns:
|
|
349
|
+
Elapsed wall-clock seconds since the browser captured this frame.
|
|
350
|
+
"""
|
|
351
|
+
return time.time() - self.capture_ts
|
|
352
|
+
|
|
353
|
+
def image(self) -> Any:
|
|
354
|
+
"""Decode the frame to a NumPy BGR image.
|
|
355
|
+
|
|
356
|
+
JPEG and raw RGBA frames are supported. Raw WebGL frames are vertically
|
|
357
|
+
flipped and converted from RGBA to BGR. H.264 decoding is reserved for a
|
|
358
|
+
future implementation.
|
|
359
|
+
|
|
360
|
+
Returns:
|
|
361
|
+
A ``height × width × 3`` uint8 BGR NumPy array. A truncated raw frame
|
|
362
|
+
returns ``None``.
|
|
363
|
+
|
|
364
|
+
Raises:
|
|
365
|
+
RuntimeError: If the optional OpenCV/NumPy dependencies are absent.
|
|
366
|
+
NotImplementedError: If the frame uses the reserved H.264 codec.
|
|
367
|
+
|
|
368
|
+
Examples:
|
|
369
|
+
Install ``sim2bot[cv2]``, then decode a received frame::
|
|
370
|
+
|
|
371
|
+
frame = stream.read(timeout=2.0)
|
|
372
|
+
if frame is not None:
|
|
373
|
+
image_bgr = frame.image()
|
|
374
|
+
"""
|
|
375
|
+
if self.codec == _CODEC_H264:
|
|
376
|
+
raise NotImplementedError("H.264 decode needs the sim2bot[av] extra.")
|
|
377
|
+
try:
|
|
378
|
+
import cv2 # type: ignore
|
|
379
|
+
import numpy as np # type: ignore
|
|
380
|
+
except ImportError as exc: # pragma: no cover - dependency hint
|
|
381
|
+
raise RuntimeError(
|
|
382
|
+
"Decoding camera frames needs OpenCV + numpy: pip install 'sim2bot[cv2]'"
|
|
383
|
+
) from exc
|
|
384
|
+
if self.codec == _CODEC_RAW:
|
|
385
|
+
# Uncompressed RGBA, bottom-up (WebGL order): reshape, flip, RGBA->BGR.
|
|
386
|
+
need = self.width * self.height * 4
|
|
387
|
+
if len(self.data) < need:
|
|
388
|
+
return None
|
|
389
|
+
arr = np.frombuffer(self.data, np.uint8, count=need)
|
|
390
|
+
img = arr.reshape(self.height, self.width, 4)[::-1]
|
|
391
|
+
return cv2.cvtColor(img, cv2.COLOR_RGBA2BGR)
|
|
392
|
+
return cv2.imdecode(np.frombuffer(self.data, np.uint8), cv2.IMREAD_COLOR)
|
|
393
|
+
|
|
394
|
+
|
|
395
|
+
def _parse_frame(data: bytes) -> Optional[CameraFrame]:
|
|
396
|
+
if len(data) < _HEADER.size + 1:
|
|
397
|
+
return None
|
|
398
|
+
version, codec, flags, sequence, capture_ts, width, height = _HEADER.unpack_from(data, 0)
|
|
399
|
+
del version
|
|
400
|
+
id_len = data[_HEADER.size]
|
|
401
|
+
start = _HEADER.size + 1 + id_len
|
|
402
|
+
if len(data) < start:
|
|
403
|
+
return None
|
|
404
|
+
camera_id = data[_HEADER.size + 1 : start].decode("utf-8", "replace")
|
|
405
|
+
return CameraFrame(
|
|
406
|
+
camera_id=camera_id,
|
|
407
|
+
codec=codec,
|
|
408
|
+
keyframe=bool(flags & _FLAG_KEYFRAME),
|
|
409
|
+
sequence=sequence,
|
|
410
|
+
capture_ts=capture_ts,
|
|
411
|
+
width=width,
|
|
412
|
+
height=height,
|
|
413
|
+
data=bytes(data[start:]),
|
|
414
|
+
)
|
|
415
|
+
|
|
416
|
+
|
|
417
|
+
def _video_url(control_url: str) -> str:
|
|
418
|
+
parsed = urlparse(control_url)
|
|
419
|
+
if parsed.scheme and parsed.netloc:
|
|
420
|
+
path = parsed.path
|
|
421
|
+
if path.endswith("/ws"):
|
|
422
|
+
path = path[: -len("/ws")] + "/video"
|
|
423
|
+
else:
|
|
424
|
+
path = path.rstrip("/") + "/video"
|
|
425
|
+
return urlunparse(parsed._replace(path=path))
|
|
426
|
+
if control_url.endswith("/ws"):
|
|
427
|
+
return control_url[: -len("/ws")] + "/video"
|
|
428
|
+
return control_url.rstrip("/") + "/video"
|
|
429
|
+
|
|
430
|
+
|
|
431
|
+
class _LatestSlot:
|
|
432
|
+
"""Holds only the newest item; readers block until something newer arrives.
|
|
433
|
+
|
|
434
|
+
This is the "always read the present" buffer — a slow reader drops the frames
|
|
435
|
+
it missed instead of building a backlog.
|
|
436
|
+
"""
|
|
437
|
+
|
|
438
|
+
def __init__(self) -> None:
|
|
439
|
+
self._cond = threading.Condition()
|
|
440
|
+
self._value: Any = None
|
|
441
|
+
self._seq = 0
|
|
442
|
+
|
|
443
|
+
def put(self, value: Any) -> None:
|
|
444
|
+
with self._cond:
|
|
445
|
+
self._value = value
|
|
446
|
+
self._seq += 1
|
|
447
|
+
self._cond.notify_all()
|
|
448
|
+
|
|
449
|
+
def get(self, last_seq: int, timeout: float) -> tuple[Any, int]:
|
|
450
|
+
with self._cond:
|
|
451
|
+
if self._seq == last_seq:
|
|
452
|
+
self._cond.wait(timeout)
|
|
453
|
+
if self._seq == last_seq:
|
|
454
|
+
return None, last_seq
|
|
455
|
+
return self._value, self._seq
|
|
456
|
+
|
|
457
|
+
|
|
458
|
+
class CameraStream:
|
|
459
|
+
"""Latest-value iterator for a subscribed global scene camera.
|
|
460
|
+
|
|
461
|
+
Use it as a context manager or call
|
|
462
|
+
[`close`][sim2bot.client.CameraStream.close] to unsubscribe. The stream
|
|
463
|
+
keeps only the newest frame, so a slow consumer drops intermediate images
|
|
464
|
+
instead of accumulating latency. Iteration also skips frames older than
|
|
465
|
+
``max_age``.
|
|
466
|
+
|
|
467
|
+
Args:
|
|
468
|
+
robot: Owning client connection. The name is historical; cameras are
|
|
469
|
+
global scene resources and are not owned by a robot command address.
|
|
470
|
+
camera_id: ID returned by [`Robot.cameras`][sim2bot.client.Robot.cameras].
|
|
471
|
+
max_age: Maximum frame age in seconds accepted by iterator mode.
|
|
472
|
+
"""
|
|
473
|
+
|
|
474
|
+
def __init__(self, robot: "Robot", camera_id: str, max_age: float = 0.25) -> None:
|
|
475
|
+
self._robot = robot
|
|
476
|
+
self.camera_id = camera_id
|
|
477
|
+
self._slot = _LatestSlot()
|
|
478
|
+
self._last_seq = 0
|
|
479
|
+
self._max_age = max_age
|
|
480
|
+
self._closed = False
|
|
481
|
+
|
|
482
|
+
def _deliver(self, frame: CameraFrame) -> None:
|
|
483
|
+
self._slot.put(frame)
|
|
484
|
+
|
|
485
|
+
def read(self, timeout: float = 5.0) -> Optional[CameraFrame]:
|
|
486
|
+
"""Wait for a frame newer than the previous read.
|
|
487
|
+
|
|
488
|
+
Args:
|
|
489
|
+
timeout: Maximum blocking time in seconds.
|
|
490
|
+
|
|
491
|
+
Returns:
|
|
492
|
+
The next [`CameraFrame`][sim2bot.client.CameraFrame], or ``None`` when
|
|
493
|
+
the timeout expires.
|
|
494
|
+
|
|
495
|
+
Notes:
|
|
496
|
+
``read()`` returns the next fresh slot value even if it is older than
|
|
497
|
+
``max_age``. Iterator mode performs the age filter automatically.
|
|
498
|
+
"""
|
|
499
|
+
frame, self._last_seq = self._slot.get(self._last_seq, timeout)
|
|
500
|
+
return frame
|
|
501
|
+
|
|
502
|
+
def __iter__(self) -> Iterator[CameraFrame]:
|
|
503
|
+
return self
|
|
504
|
+
|
|
505
|
+
def __next__(self) -> CameraFrame:
|
|
506
|
+
while not self._closed:
|
|
507
|
+
frame = self.read()
|
|
508
|
+
if frame is None:
|
|
509
|
+
continue
|
|
510
|
+
# Skip a frame that's already stale (a fresher one is on the way).
|
|
511
|
+
if frame.age > self._max_age:
|
|
512
|
+
continue
|
|
513
|
+
return frame
|
|
514
|
+
raise StopIteration
|
|
515
|
+
|
|
516
|
+
def close(self) -> None:
|
|
517
|
+
"""Unsubscribe this camera and make the operation idempotently closed.
|
|
518
|
+
|
|
519
|
+
Returns:
|
|
520
|
+
Calling the method again after closure has no effect.
|
|
521
|
+
"""
|
|
522
|
+
if self._closed:
|
|
523
|
+
return
|
|
524
|
+
self._closed = True
|
|
525
|
+
self._robot._remove_camera(self.camera_id)
|
|
526
|
+
|
|
527
|
+
def __enter__(self) -> "CameraStream":
|
|
528
|
+
return self
|
|
529
|
+
|
|
530
|
+
def __exit__(self, *exc: Any) -> None:
|
|
531
|
+
self.close()
|
|
532
|
+
|
|
533
|
+
|
|
534
|
+
class Robot:
|
|
535
|
+
"""A Sim2Bot robot endpoint over the local bridge.
|
|
536
|
+
|
|
537
|
+
Args:
|
|
538
|
+
url: Control WebSocket URL. Defaults to ``ws://localhost:8765/ws``.
|
|
539
|
+
video_url: Binary camera WebSocket URL. Derived from ``url`` when omitted.
|
|
540
|
+
connect_timeout: WebSocket connection timeout in seconds.
|
|
541
|
+
transport: ``"ws"`` for reliable WebSocket control/telemetry or ``"udp"``
|
|
542
|
+
for latest-value UDP. Camera frames always use the video WebSocket.
|
|
543
|
+
udp_host: UDP bridge host. Derived from ``url`` when omitted.
|
|
544
|
+
udp_port: UDP bridge port. Defaults to 8771.
|
|
545
|
+
auto_bridge: Start a local bridge automatically when none is healthy.
|
|
546
|
+
bridge_timeout: Seconds to wait for an automatically started bridge.
|
|
547
|
+
wait_for_sim: During [`connect`][sim2bot.client.Robot.connect], wait for a
|
|
548
|
+
browser scene.
|
|
549
|
+
wait_for_sim_timeout: Maximum wait in seconds, or ``None`` to wait forever.
|
|
550
|
+
wait_for_sim_poll_interval: Delay between scene discovery attempts.
|
|
551
|
+
api_key: Optional bridge token for deliberately enabled LAN access. It is
|
|
552
|
+
not needed for normal same-device loopback use.
|
|
553
|
+
room: Optional pairing ID. Controllers communicate only with browser
|
|
554
|
+
simulators in the same room.
|
|
555
|
+
|
|
556
|
+
Notes:
|
|
557
|
+
This client is blocking and thread-based. Telemetry and camera streams
|
|
558
|
+
deliberately retain only the newest value. It is designed for simulation
|
|
559
|
+
and is not a functional-safety interface for physical hardware.
|
|
560
|
+
|
|
561
|
+
Examples:
|
|
562
|
+
Use the client as a context manager so connections always close::
|
|
563
|
+
|
|
564
|
+
from sim2bot import Robot
|
|
565
|
+
|
|
566
|
+
with Robot(auto_bridge=True, wait_for_sim=True) as sim:
|
|
567
|
+
arm = sim.describe()[0]
|
|
568
|
+
sim.move_to(arm.home, robot=arm.index)
|
|
569
|
+
"""
|
|
570
|
+
|
|
571
|
+
def __init__(
|
|
572
|
+
self,
|
|
573
|
+
url: str = "ws://localhost:8765/ws",
|
|
574
|
+
video_url: Optional[str] = None,
|
|
575
|
+
connect_timeout: float = 5.0,
|
|
576
|
+
transport: str = "ws",
|
|
577
|
+
udp_host: Optional[str] = None,
|
|
578
|
+
udp_port: int = 8771,
|
|
579
|
+
auto_bridge: bool = False,
|
|
580
|
+
bridge_timeout: float = 10.0,
|
|
581
|
+
wait_for_sim: bool = False,
|
|
582
|
+
wait_for_sim_timeout: Optional[float] = None,
|
|
583
|
+
wait_for_sim_poll_interval: float = 0.5,
|
|
584
|
+
api_key: Optional[str] = None,
|
|
585
|
+
room: Optional[str] = None,
|
|
586
|
+
) -> None:
|
|
587
|
+
self.url = url
|
|
588
|
+
self.video_url = video_url or _video_url(url)
|
|
589
|
+
self._connect_timeout = connect_timeout
|
|
590
|
+
self._auto_bridge = auto_bridge
|
|
591
|
+
self._bridge_timeout = bridge_timeout
|
|
592
|
+
self._wait_for_sim_on_connect = wait_for_sim
|
|
593
|
+
self._wait_for_sim_timeout = wait_for_sim_timeout
|
|
594
|
+
self._wait_for_sim_poll_interval = wait_for_sim_poll_interval
|
|
595
|
+
self._api_key = api_key
|
|
596
|
+
self._room = room or os.environ.get("SIM2BOT_BRIDGE_ROOM")
|
|
597
|
+
# Control + telemetry transport: "ws" (WebSocket/TCP, default) or "udp"
|
|
598
|
+
# (lower-latency, drop-don't-queue — best for tight control loops). The
|
|
599
|
+
# camera feed always uses the binary video WebSocket regardless.
|
|
600
|
+
self.transport = transport
|
|
601
|
+
host = udp_host or urlparse(url).hostname or "127.0.0.1"
|
|
602
|
+
self._udp_dest = (host, udp_port)
|
|
603
|
+
|
|
604
|
+
self._conn = None # WebSocket control connection (ws transport)
|
|
605
|
+
self._udp_sock: Optional[socket.socket] = None # UDP control socket
|
|
606
|
+
self._video_conn = None
|
|
607
|
+
self._send_lock = threading.Lock()
|
|
608
|
+
self._stop = threading.Event()
|
|
609
|
+
|
|
610
|
+
self._states: dict[int, RobotState] = {}
|
|
611
|
+
self._states_lock = threading.Lock()
|
|
612
|
+
self._on_telemetry: Optional[Callable[[RobotState], None]] = None
|
|
613
|
+
|
|
614
|
+
self._scene: Optional[dict] = None
|
|
615
|
+
self._scene_event = threading.Event()
|
|
616
|
+
|
|
617
|
+
# camera_id -> (options dict, CameraStream)
|
|
618
|
+
self._cameras: dict[str, tuple[dict, CameraStream]] = {}
|
|
619
|
+
self._cameras_lock = threading.Lock()
|
|
620
|
+
|
|
621
|
+
self._threads: list[threading.Thread] = []
|
|
622
|
+
|
|
623
|
+
# -- lifecycle -----------------------------------------------------------
|
|
624
|
+
|
|
625
|
+
def connect(self) -> "Robot":
|
|
626
|
+
"""Open the configured bridge connection and start reader threads.
|
|
627
|
+
|
|
628
|
+
If ``auto_bridge=True``, this first reuses a healthy local bridge or starts
|
|
629
|
+
one. If ``wait_for_sim=True``, the call does not return until a browser
|
|
630
|
+
announces at least one robot or the configured timeout expires.
|
|
631
|
+
|
|
632
|
+
Returns:
|
|
633
|
+
This client instance, enabling ``Robot(...).connect()`` chaining.
|
|
634
|
+
|
|
635
|
+
Raises:
|
|
636
|
+
TimeoutError: If waiting for a browser simulator times out.
|
|
637
|
+
RuntimeError: If an automatically started bridge cannot become healthy.
|
|
638
|
+
ConnectionClosed: If the WebSocket closes while sending its initial hello.
|
|
639
|
+
OSError: If the configured network endpoint cannot be opened.
|
|
640
|
+
"""
|
|
641
|
+
if self._auto_bridge:
|
|
642
|
+
from .bridge import ensure_bridge
|
|
643
|
+
|
|
644
|
+
info = ensure_bridge(
|
|
645
|
+
self.url,
|
|
646
|
+
udp_port=self._udp_dest[1],
|
|
647
|
+
timeout=self._bridge_timeout,
|
|
648
|
+
)
|
|
649
|
+
self.url = info.url
|
|
650
|
+
self.video_url = self.video_url or info.video_url
|
|
651
|
+
if self.transport == "udp":
|
|
652
|
+
self._udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
653
|
+
self._udp_sock.settimeout(1.0)
|
|
654
|
+
# A first datagram registers us with the bridge so telemetry flows
|
|
655
|
+
# back to this socket (the bridge replies to the sender address).
|
|
656
|
+
self._send({"type": "describe"})
|
|
657
|
+
self._spawn(self._control_reader_udp, name="sim2bot-control-udp")
|
|
658
|
+
else:
|
|
659
|
+
self._conn = ws_connect(
|
|
660
|
+
self.url,
|
|
661
|
+
open_timeout=self._connect_timeout,
|
|
662
|
+
additional_headers=self._auth_headers(),
|
|
663
|
+
)
|
|
664
|
+
self._send({"type": "hello", "role": "controller"})
|
|
665
|
+
self._spawn(self._control_reader_ws, name="sim2bot-control")
|
|
666
|
+
self._spawn(self._renew_loop, name="sim2bot-renew")
|
|
667
|
+
if self._wait_for_sim_on_connect:
|
|
668
|
+
self.wait_for_sim(
|
|
669
|
+
timeout=self._wait_for_sim_timeout,
|
|
670
|
+
poll_interval=self._wait_for_sim_poll_interval,
|
|
671
|
+
)
|
|
672
|
+
return self
|
|
673
|
+
|
|
674
|
+
def close(self) -> None:
|
|
675
|
+
"""Close control, telemetry, camera, and UDP resources.
|
|
676
|
+
|
|
677
|
+
Active camera feeds are unsubscribed first. Calling ``close()`` more than
|
|
678
|
+
once is safe.
|
|
679
|
+
|
|
680
|
+
Returns:
|
|
681
|
+
The method is safe to call repeatedly.
|
|
682
|
+
"""
|
|
683
|
+
self._stop.set()
|
|
684
|
+
with self._cameras_lock:
|
|
685
|
+
camera_ids = list(self._cameras.keys())
|
|
686
|
+
for camera_id in camera_ids:
|
|
687
|
+
self._unsubscribe(camera_id)
|
|
688
|
+
for conn in (self._conn, self._video_conn, self._udp_sock):
|
|
689
|
+
if conn is not None:
|
|
690
|
+
try:
|
|
691
|
+
conn.close()
|
|
692
|
+
except Exception:
|
|
693
|
+
pass
|
|
694
|
+
self._conn = None
|
|
695
|
+
self._video_conn = None
|
|
696
|
+
self._udp_sock = None
|
|
697
|
+
|
|
698
|
+
def __enter__(self) -> "Robot":
|
|
699
|
+
return self.connect()
|
|
700
|
+
|
|
701
|
+
def __exit__(self, *exc: Any) -> None:
|
|
702
|
+
self.close()
|
|
703
|
+
|
|
704
|
+
def _spawn(self, target, name: str) -> None:
|
|
705
|
+
thread = threading.Thread(target=target, name=name, daemon=True)
|
|
706
|
+
thread.start()
|
|
707
|
+
self._threads.append(thread)
|
|
708
|
+
|
|
709
|
+
def _send(self, message: dict) -> None:
|
|
710
|
+
if self._room:
|
|
711
|
+
message = {**message, "room": self._room}
|
|
712
|
+
if self._api_key and self.transport == "udp":
|
|
713
|
+
message = {**message, "authToken": self._api_key}
|
|
714
|
+
payload = json.dumps(message)
|
|
715
|
+
if self.transport == "udp":
|
|
716
|
+
if self._udp_sock is None:
|
|
717
|
+
raise RuntimeError("not connected — call connect() first")
|
|
718
|
+
self._udp_sock.sendto(payload.encode(), self._udp_dest)
|
|
719
|
+
return
|
|
720
|
+
if self._conn is None:
|
|
721
|
+
raise RuntimeError("not connected — call connect() first")
|
|
722
|
+
with self._send_lock:
|
|
723
|
+
self._conn.send(payload)
|
|
724
|
+
|
|
725
|
+
# -- control readers -----------------------------------------------------
|
|
726
|
+
|
|
727
|
+
def _handle_control_message(self, message: dict) -> None:
|
|
728
|
+
kind = message.get("type")
|
|
729
|
+
if kind == "telemetry":
|
|
730
|
+
state = RobotState.from_json(message)
|
|
731
|
+
with self._states_lock:
|
|
732
|
+
self._states[state.robot] = state
|
|
733
|
+
if self._on_telemetry is not None:
|
|
734
|
+
self._on_telemetry(state) # one call per received packet
|
|
735
|
+
elif kind == "scene":
|
|
736
|
+
self._scene = message
|
|
737
|
+
self._scene_event.set()
|
|
738
|
+
|
|
739
|
+
def _control_reader_ws(self) -> None:
|
|
740
|
+
conn = self._conn
|
|
741
|
+
if conn is None:
|
|
742
|
+
return
|
|
743
|
+
while not self._stop.is_set():
|
|
744
|
+
try:
|
|
745
|
+
raw = conn.recv(timeout=1.0)
|
|
746
|
+
except TimeoutError:
|
|
747
|
+
continue
|
|
748
|
+
except ConnectionClosed:
|
|
749
|
+
break
|
|
750
|
+
if isinstance(raw, bytes):
|
|
751
|
+
continue
|
|
752
|
+
try:
|
|
753
|
+
message = json.loads(raw)
|
|
754
|
+
except ValueError:
|
|
755
|
+
continue
|
|
756
|
+
self._handle_control_message(message)
|
|
757
|
+
|
|
758
|
+
def _control_reader_udp(self) -> None:
|
|
759
|
+
sock = self._udp_sock
|
|
760
|
+
if sock is None:
|
|
761
|
+
return
|
|
762
|
+
while not self._stop.is_set():
|
|
763
|
+
try:
|
|
764
|
+
data, _ = sock.recvfrom(65535)
|
|
765
|
+
except socket.timeout:
|
|
766
|
+
continue
|
|
767
|
+
except OSError:
|
|
768
|
+
break
|
|
769
|
+
try:
|
|
770
|
+
message = json.loads(data.decode())
|
|
771
|
+
except ValueError:
|
|
772
|
+
continue
|
|
773
|
+
if isinstance(message, dict):
|
|
774
|
+
self._handle_control_message(message)
|
|
775
|
+
|
|
776
|
+
# -- discovery + telemetry ----------------------------------------------
|
|
777
|
+
|
|
778
|
+
def describe(self, timeout: float = 2.0) -> list[RobotInfo]:
|
|
779
|
+
"""Request model-derived metadata for every robot in the current scene.
|
|
780
|
+
|
|
781
|
+
Args:
|
|
782
|
+
timeout: Maximum time in seconds to wait for a scene announcement.
|
|
783
|
+
|
|
784
|
+
Returns:
|
|
785
|
+
Robots in current scene order. The returned ``index`` is the command
|
|
786
|
+
address to pass as ``robot=``. An empty list means no scene response
|
|
787
|
+
arrived before the timeout or no robot is loaded.
|
|
788
|
+
|
|
789
|
+
Raises:
|
|
790
|
+
RuntimeError: If the client is not connected.
|
|
791
|
+
ConnectionClosed: If the WebSocket closes while sending discovery.
|
|
792
|
+
OSError: If the selected transport cannot send the request.
|
|
793
|
+
|
|
794
|
+
Notes:
|
|
795
|
+
Discover the scene instead of hard-coding degree of freedom, joint
|
|
796
|
+
order, limits, or home targets.
|
|
797
|
+
"""
|
|
798
|
+
self._scene_event.clear()
|
|
799
|
+
self._send({"type": "describe"})
|
|
800
|
+
self._scene_event.wait(timeout)
|
|
801
|
+
robots = (self._scene or {}).get("robots", [])
|
|
802
|
+
return [RobotInfo.from_json(item) for item in robots]
|
|
803
|
+
|
|
804
|
+
def wait_for_sim(
|
|
805
|
+
self,
|
|
806
|
+
timeout: Optional[float] = None,
|
|
807
|
+
poll_interval: float = 0.5,
|
|
808
|
+
) -> list[RobotInfo]:
|
|
809
|
+
"""Wait until a browser simulator connects and announces robots.
|
|
810
|
+
|
|
811
|
+
Args:
|
|
812
|
+
timeout: Maximum total wait in seconds, or ``None`` to wait forever.
|
|
813
|
+
poll_interval: Delay in seconds between unsuccessful discovery calls.
|
|
814
|
+
|
|
815
|
+
Returns:
|
|
816
|
+
The non-empty list of discovered
|
|
817
|
+
[`RobotInfo`][sim2bot.client.RobotInfo] objects.
|
|
818
|
+
|
|
819
|
+
Raises:
|
|
820
|
+
TimeoutError: If no browser scene appears before ``timeout``.
|
|
821
|
+
RuntimeError: If the client is not connected.
|
|
822
|
+
ConnectionClosed: If the WebSocket closes during discovery.
|
|
823
|
+
OSError: If the selected transport cannot send a discovery request.
|
|
824
|
+
|
|
825
|
+
Examples:
|
|
826
|
+
This allows a controller to start before the browser::
|
|
827
|
+
|
|
828
|
+
with Robot(auto_bridge=True, wait_for_sim=True) as sim:
|
|
829
|
+
robots = sim.describe()
|
|
830
|
+
"""
|
|
831
|
+
deadline = None if timeout is None else time.time() + timeout
|
|
832
|
+
while True:
|
|
833
|
+
describe_timeout = 1.0
|
|
834
|
+
if deadline is not None:
|
|
835
|
+
remaining = deadline - time.time()
|
|
836
|
+
if remaining <= 0:
|
|
837
|
+
raise TimeoutError("Timed out waiting for Sim2Bot browser simulator")
|
|
838
|
+
describe_timeout = max(0.05, min(describe_timeout, remaining))
|
|
839
|
+
|
|
840
|
+
robots = self.describe(timeout=describe_timeout)
|
|
841
|
+
if robots:
|
|
842
|
+
return robots
|
|
843
|
+
|
|
844
|
+
if deadline is not None:
|
|
845
|
+
remaining = deadline - time.time()
|
|
846
|
+
if remaining <= 0:
|
|
847
|
+
raise TimeoutError("Timed out waiting for Sim2Bot browser simulator")
|
|
848
|
+
time.sleep(min(poll_interval, remaining))
|
|
849
|
+
else:
|
|
850
|
+
time.sleep(poll_interval)
|
|
851
|
+
|
|
852
|
+
def cameras(self, timeout: float = 2.0) -> list[dict]:
|
|
853
|
+
"""Return the global scene cameras available for subscription.
|
|
854
|
+
|
|
855
|
+
Args:
|
|
856
|
+
timeout: Discovery timeout in seconds when the scene is not cached.
|
|
857
|
+
|
|
858
|
+
Returns:
|
|
859
|
+
Camera dictionaries containing an ``id``, label, kind, stream
|
|
860
|
+
defaults, and mount metadata. Mount kind may be ``world``, ``robot``,
|
|
861
|
+
``object``, or ``sensor``.
|
|
862
|
+
|
|
863
|
+
Raises:
|
|
864
|
+
RuntimeError: If discovery is needed and the client is not connected.
|
|
865
|
+
ConnectionClosed: If the WebSocket closes during discovery.
|
|
866
|
+
OSError: If the selected transport cannot send a discovery request.
|
|
867
|
+
|
|
868
|
+
Notes:
|
|
869
|
+
Camera IDs are global scene resources. A world-mounted overhead camera
|
|
870
|
+
can observe several robots and is not addressed with ``robot=``.
|
|
871
|
+
"""
|
|
872
|
+
if self._scene is None:
|
|
873
|
+
self.describe(timeout)
|
|
874
|
+
return list((self._scene or {}).get("cameras", []))
|
|
875
|
+
|
|
876
|
+
def room_devices(self, timeout: float = 2.0) -> list[RoomDeviceInfo]:
|
|
877
|
+
"""Return authored doors and windows available for scene-level control.
|
|
878
|
+
|
|
879
|
+
Args:
|
|
880
|
+
timeout: Discovery timeout in seconds when the scene is not cached.
|
|
881
|
+
|
|
882
|
+
Returns:
|
|
883
|
+
Discovered [`RoomDeviceInfo`][sim2bot.client.RoomDeviceInfo] entries.
|
|
884
|
+
|
|
885
|
+
Raises:
|
|
886
|
+
RuntimeError: If discovery is needed and the client is not connected.
|
|
887
|
+
ConnectionClosed: If the WebSocket closes during discovery.
|
|
888
|
+
OSError: If the selected transport cannot send a discovery request.
|
|
889
|
+
|
|
890
|
+
Notes:
|
|
891
|
+
Room actuation is experimental while browser-side mechanisms are
|
|
892
|
+
being revised.
|
|
893
|
+
"""
|
|
894
|
+
if self._scene is None:
|
|
895
|
+
self.describe(timeout)
|
|
896
|
+
return [
|
|
897
|
+
RoomDeviceInfo.from_json(item)
|
|
898
|
+
for item in (self._scene or {}).get("devices", [])
|
|
899
|
+
]
|
|
900
|
+
|
|
901
|
+
def state(self, robot: int = 0) -> Optional[RobotState]:
|
|
902
|
+
"""Return the newest complete telemetry packet for one robot.
|
|
903
|
+
|
|
904
|
+
Args:
|
|
905
|
+
robot: Robot index returned by
|
|
906
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
907
|
+
|
|
908
|
+
Returns:
|
|
909
|
+
Latest [`RobotState`][sim2bot.client.RobotState], or ``None`` before
|
|
910
|
+
the first packet arrives.
|
|
911
|
+
|
|
912
|
+
Notes:
|
|
913
|
+
State is latest-value rather than queued. A slow consumer does not
|
|
914
|
+
fall behind by reading old packets.
|
|
915
|
+
"""
|
|
916
|
+
with self._states_lock:
|
|
917
|
+
return self._states.get(robot)
|
|
918
|
+
|
|
919
|
+
def on_telemetry(self, callback: Optional[Callable[[RobotState], None]]) -> None:
|
|
920
|
+
"""Register or clear a callback for every received robot-state packet.
|
|
921
|
+
|
|
922
|
+
Args:
|
|
923
|
+
callback: Function receiving one
|
|
924
|
+
[`RobotState`][sim2bot.client.RobotState], or ``None`` to unregister
|
|
925
|
+
the current callback.
|
|
926
|
+
|
|
927
|
+
Returns:
|
|
928
|
+
Registration takes effect immediately.
|
|
929
|
+
|
|
930
|
+
Warning:
|
|
931
|
+
The callback runs on the SDK reader thread. Keep it short and
|
|
932
|
+
thread-safe; hand work to your own queue rather than blocking it.
|
|
933
|
+
"""
|
|
934
|
+
self._on_telemetry = callback
|
|
935
|
+
|
|
936
|
+
def states(self, robot: int = 0) -> list[dict]:
|
|
937
|
+
"""Return physics-substep samples carried by the newest telemetry packet.
|
|
938
|
+
|
|
939
|
+
Args:
|
|
940
|
+
robot: Robot index returned by
|
|
941
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
942
|
+
|
|
943
|
+
Returns:
|
|
944
|
+
A list of ``{"t": seconds, "q": radians, "qd": radians_per_second}``
|
|
945
|
+
dictionaries, or an empty list before telemetry arrives.
|
|
946
|
+
|
|
947
|
+
Notes:
|
|
948
|
+
The batch preserves model timestep resolution between lower-rate
|
|
949
|
+
packets. It does not turn browser delivery into a deterministic
|
|
950
|
+
wall-clock 500 Hz stream.
|
|
951
|
+
"""
|
|
952
|
+
state = self.state(robot)
|
|
953
|
+
return state.samples if state else []
|
|
954
|
+
|
|
955
|
+
def wait_until_reached(
|
|
956
|
+
self,
|
|
957
|
+
q: Sequence[float],
|
|
958
|
+
robot: int = 0,
|
|
959
|
+
tol: float = 0.02,
|
|
960
|
+
timeout: float = 10.0,
|
|
961
|
+
) -> bool:
|
|
962
|
+
"""Wait until every addressed joint is within a target tolerance.
|
|
963
|
+
|
|
964
|
+
Args:
|
|
965
|
+
q: Target joint positions in radians and discovered joint order.
|
|
966
|
+
robot: Robot index returned by
|
|
967
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
968
|
+
tol: Absolute per-joint tolerance in radians.
|
|
969
|
+
timeout: Maximum wait in seconds.
|
|
970
|
+
|
|
971
|
+
Returns:
|
|
972
|
+
``True`` when all supplied joints reach tolerance; ``False`` on timeout.
|
|
973
|
+
"""
|
|
974
|
+
deadline = time.time() + timeout
|
|
975
|
+
while time.time() < deadline:
|
|
976
|
+
state = self.state(robot)
|
|
977
|
+
if state and state.q and len(state.q) >= len(q):
|
|
978
|
+
if all(abs(state.q[i] - q[i]) <= tol for i in range(len(q))):
|
|
979
|
+
return True
|
|
980
|
+
time.sleep(0.02)
|
|
981
|
+
return False
|
|
982
|
+
|
|
983
|
+
# -- commands ------------------------------------------------------------
|
|
984
|
+
|
|
985
|
+
def move_to(self, q: Sequence[float], robot: int = 0) -> None:
|
|
986
|
+
"""Send a joint-position target to one robot.
|
|
987
|
+
|
|
988
|
+
Args:
|
|
989
|
+
q: Joint positions in radians, ordered exactly like
|
|
990
|
+
`RobotInfo.joint_names`. Normally supply ``dof`` values.
|
|
991
|
+
robot: Robot index returned by
|
|
992
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
993
|
+
|
|
994
|
+
Returns:
|
|
995
|
+
The target is sent asynchronously to the simulator.
|
|
996
|
+
|
|
997
|
+
Raises:
|
|
998
|
+
RuntimeError: If the client is not connected.
|
|
999
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1000
|
+
OSError: If the selected transport cannot send the command.
|
|
1001
|
+
|
|
1002
|
+
Notes:
|
|
1003
|
+
The command is latest-writer-wins. In physics mode the robot's
|
|
1004
|
+
controller tracks the target; in kinematics-only mode it is applied
|
|
1005
|
+
directly. Sending a new joint, velocity, TCP, stop, reset, or
|
|
1006
|
+
trajectory command replaces the prior external motion mode.
|
|
1007
|
+
|
|
1008
|
+
Warning:
|
|
1009
|
+
Use discovered limits and a model-appropriate trajectory. This method
|
|
1010
|
+
does not plan around collisions or guarantee a safe path.
|
|
1011
|
+
|
|
1012
|
+
Examples:
|
|
1013
|
+
Move the first discovered robot to its model-defined home::
|
|
1014
|
+
|
|
1015
|
+
arm = sim.describe()[0]
|
|
1016
|
+
sim.move_to(arm.home, robot=arm.index)
|
|
1017
|
+
"""
|
|
1018
|
+
self._send({"type": "joint_position", "q": list(q), "robot": robot})
|
|
1019
|
+
|
|
1020
|
+
def set_velocity(self, qd: Sequence[float], robot: int = 0) -> None:
|
|
1021
|
+
"""Stream a joint-velocity target to one robot.
|
|
1022
|
+
|
|
1023
|
+
Args:
|
|
1024
|
+
qd: Joint velocities in radians per second and discovered joint order.
|
|
1025
|
+
robot: Robot index returned by
|
|
1026
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1027
|
+
|
|
1028
|
+
Returns:
|
|
1029
|
+
The velocity target is sent asynchronously.
|
|
1030
|
+
|
|
1031
|
+
Raises:
|
|
1032
|
+
RuntimeError: If the client is not connected.
|
|
1033
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1034
|
+
OSError: If the selected transport cannot send the command.
|
|
1035
|
+
|
|
1036
|
+
Notes:
|
|
1037
|
+
Velocity control has a 500 ms safety watchdog. Refresh the command
|
|
1038
|
+
faster than that while motion should continue; stale targets expire.
|
|
1039
|
+
Commands may be sent faster than the browser applies them, in which
|
|
1040
|
+
case the newest value wins.
|
|
1041
|
+
|
|
1042
|
+
Warning:
|
|
1043
|
+
This command does not generate collision-free motion. Stop streaming
|
|
1044
|
+
or call [`stop`][sim2bot.client.Robot.stop] before leaving a control
|
|
1045
|
+
loop.
|
|
1046
|
+
|
|
1047
|
+
Examples:
|
|
1048
|
+
Stream a small velocity for 250 ms, then hold::
|
|
1049
|
+
|
|
1050
|
+
sim.set_velocity([0.1] + [0.0] * 6)
|
|
1051
|
+
time.sleep(0.25)
|
|
1052
|
+
sim.stop()
|
|
1053
|
+
"""
|
|
1054
|
+
self._send({"type": "joint_velocity", "qd": list(qd), "robot": robot})
|
|
1055
|
+
|
|
1056
|
+
def move_to_pose(
|
|
1057
|
+
self,
|
|
1058
|
+
position: Sequence[float],
|
|
1059
|
+
orientation: Optional[Sequence[float]] = None,
|
|
1060
|
+
robot: int = 0,
|
|
1061
|
+
) -> None:
|
|
1062
|
+
"""Command a Cartesian TCP target solved by in-browser inverse kinematics.
|
|
1063
|
+
|
|
1064
|
+
Args:
|
|
1065
|
+
position: World-frame ``[x, y, z]`` in metres.
|
|
1066
|
+
orientation: Optional world-frame quaternion ``[x, y, z, w]``. Omit
|
|
1067
|
+
it for position-only IK.
|
|
1068
|
+
robot: Robot index returned by
|
|
1069
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1070
|
+
|
|
1071
|
+
Returns:
|
|
1072
|
+
The pose target is sent to the browser IK controller.
|
|
1073
|
+
|
|
1074
|
+
Raises:
|
|
1075
|
+
TypeError: If position or orientation is not an iterable of numbers.
|
|
1076
|
+
ValueError: If a supplied component cannot be converted to ``float``.
|
|
1077
|
+
RuntimeError: If the client is not connected.
|
|
1078
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1079
|
+
OSError: If the selected transport cannot send the command.
|
|
1080
|
+
|
|
1081
|
+
Notes:
|
|
1082
|
+
The IK solution drives the same external target path as joint control.
|
|
1083
|
+
Unreachable targets may settle at the closest configuration the solver
|
|
1084
|
+
finds; verify `RobotState.tcp` before continuing.
|
|
1085
|
+
|
|
1086
|
+
Warning:
|
|
1087
|
+
IK does not imply a collision-free path and can choose a different
|
|
1088
|
+
joint configuration near singularities.
|
|
1089
|
+
|
|
1090
|
+
Examples:
|
|
1091
|
+
Send a position-only target, then a full pose::
|
|
1092
|
+
|
|
1093
|
+
sim.move_to_pose([0.45, 0.0, 0.35])
|
|
1094
|
+
sim.move_to_pose([0.45, 0.0, 0.35], [0.0, 0.0, 0.0, 1.0])
|
|
1095
|
+
"""
|
|
1096
|
+
message: dict = {
|
|
1097
|
+
"type": "tcp_pose",
|
|
1098
|
+
"position": [float(v) for v in position],
|
|
1099
|
+
"robot": robot,
|
|
1100
|
+
}
|
|
1101
|
+
if orientation is not None:
|
|
1102
|
+
message["orientation"] = [float(v) for v in orientation]
|
|
1103
|
+
self._send(message)
|
|
1104
|
+
|
|
1105
|
+
def gripper(self, fraction: float, robot: int = 0) -> None:
|
|
1106
|
+
"""Set normalized gripper openness for one robot.
|
|
1107
|
+
|
|
1108
|
+
Args:
|
|
1109
|
+
fraction: ``0.0`` fully closed through ``1.0`` fully open.
|
|
1110
|
+
robot: Robot index returned by
|
|
1111
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1112
|
+
|
|
1113
|
+
Returns:
|
|
1114
|
+
Unsupported grippers may ignore the command.
|
|
1115
|
+
|
|
1116
|
+
Raises:
|
|
1117
|
+
TypeError: If ``fraction`` cannot be converted to ``float``.
|
|
1118
|
+
ValueError: If ``fraction`` is not a numeric value.
|
|
1119
|
+
RuntimeError: If the client is not connected.
|
|
1120
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1121
|
+
OSError: If the selected transport cannot send the command.
|
|
1122
|
+
|
|
1123
|
+
Notes:
|
|
1124
|
+
Robots without a supported gripper ignore the command. Discover
|
|
1125
|
+
`RobotInfo.has_gripper` before relying on it.
|
|
1126
|
+
"""
|
|
1127
|
+
self._send({"type": "gripper", "fraction": float(fraction), "robot": robot})
|
|
1128
|
+
|
|
1129
|
+
def set_room_opening(self, opening: str, fraction: float) -> None:
|
|
1130
|
+
"""Set the target opening fraction of a scene door or window.
|
|
1131
|
+
|
|
1132
|
+
Args:
|
|
1133
|
+
opening: Device ID returned by
|
|
1134
|
+
[`room_devices`][sim2bot.client.Robot.room_devices].
|
|
1135
|
+
fraction: ``0.0`` closed through ``1.0`` fully open. Values are clamped.
|
|
1136
|
+
|
|
1137
|
+
Returns:
|
|
1138
|
+
The clamped target is sent asynchronously.
|
|
1139
|
+
|
|
1140
|
+
Raises:
|
|
1141
|
+
TypeError: If ``fraction`` cannot be converted to ``float``.
|
|
1142
|
+
ValueError: If ``fraction`` is not a numeric value.
|
|
1143
|
+
RuntimeError: If the client is not connected.
|
|
1144
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1145
|
+
OSError: If the selected transport cannot send the command.
|
|
1146
|
+
|
|
1147
|
+
Warning:
|
|
1148
|
+
This API is experimental. Browser-side room actuation is currently
|
|
1149
|
+
under revision and may not reliably move every authored mechanism.
|
|
1150
|
+
Do not rely on it for automated tests until that work is completed.
|
|
1151
|
+
"""
|
|
1152
|
+
value = max(0.0, min(1.0, float(fraction)))
|
|
1153
|
+
self._send(
|
|
1154
|
+
{
|
|
1155
|
+
"type": "room_opening",
|
|
1156
|
+
"opening": str(opening),
|
|
1157
|
+
"fraction": value,
|
|
1158
|
+
}
|
|
1159
|
+
)
|
|
1160
|
+
|
|
1161
|
+
def base_velocity(
|
|
1162
|
+
self, vx: float = 0.0, vy: float = 0.0, vz: float = 0.0, omega: float = 0.0,
|
|
1163
|
+
robot: int = 0,
|
|
1164
|
+
) -> None:
|
|
1165
|
+
"""Command a supported mobile or aerial base velocity.
|
|
1166
|
+
|
|
1167
|
+
Args:
|
|
1168
|
+
vx: Forward robot-frame linear velocity in metres per second.
|
|
1169
|
+
vy: Leftward robot-frame linear velocity in metres per second.
|
|
1170
|
+
vz: Vertical velocity in metres per second for aerial bases.
|
|
1171
|
+
omega: Yaw rate in radians per second.
|
|
1172
|
+
robot: Intended robot index.
|
|
1173
|
+
|
|
1174
|
+
Returns:
|
|
1175
|
+
The latest velocity command replaces the previous one.
|
|
1176
|
+
|
|
1177
|
+
Raises:
|
|
1178
|
+
RuntimeError: If the client is not connected.
|
|
1179
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1180
|
+
OSError: If the selected transport cannot send the command.
|
|
1181
|
+
|
|
1182
|
+
Warning:
|
|
1183
|
+
Multi-base addressing is not complete. The current browser runtime
|
|
1184
|
+
drives the scene's primary base even when another ``robot`` index is
|
|
1185
|
+
supplied. Treat this API as preview for multi-robot scenes.
|
|
1186
|
+
"""
|
|
1187
|
+
self._send(
|
|
1188
|
+
{
|
|
1189
|
+
"type": "base_velocity",
|
|
1190
|
+
"vx": vx, "vy": vy, "vz": vz, "omega": omega,
|
|
1191
|
+
"robot": robot,
|
|
1192
|
+
}
|
|
1193
|
+
)
|
|
1194
|
+
|
|
1195
|
+
def reset(self, robot: int = 0) -> None:
|
|
1196
|
+
"""Reset simulation state and release an addressed external command.
|
|
1197
|
+
|
|
1198
|
+
Args:
|
|
1199
|
+
robot: Robot whose external command ownership should be released.
|
|
1200
|
+
|
|
1201
|
+
Returns:
|
|
1202
|
+
Reset is requested asynchronously.
|
|
1203
|
+
|
|
1204
|
+
Raises:
|
|
1205
|
+
RuntimeError: If the client is not connected.
|
|
1206
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1207
|
+
OSError: If the selected transport cannot send the command.
|
|
1208
|
+
|
|
1209
|
+
Warning:
|
|
1210
|
+
The current MuJoCo reset is scene-wide, so other robots and dynamic
|
|
1211
|
+
objects are also reset even though command release is addressed.
|
|
1212
|
+
"""
|
|
1213
|
+
self._send({"type": "reset", "robot": robot})
|
|
1214
|
+
|
|
1215
|
+
def stop(self, robot: int = 0) -> None:
|
|
1216
|
+
"""Hold one robot at its current joint pose.
|
|
1217
|
+
|
|
1218
|
+
Args:
|
|
1219
|
+
robot: Robot index returned by
|
|
1220
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1221
|
+
|
|
1222
|
+
Returns:
|
|
1223
|
+
The simulator receives a hold-position command.
|
|
1224
|
+
|
|
1225
|
+
Raises:
|
|
1226
|
+
RuntimeError: If the client is not connected.
|
|
1227
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1228
|
+
OSError: If the selected transport cannot send the command.
|
|
1229
|
+
|
|
1230
|
+
Notes:
|
|
1231
|
+
This replaces active velocity, TCP, or trajectory control for the
|
|
1232
|
+
addressed robot. It is a simulation hold command, not an emergency
|
|
1233
|
+
stop certified for real hardware.
|
|
1234
|
+
"""
|
|
1235
|
+
self._send({"type": "stop", "robot": robot})
|
|
1236
|
+
|
|
1237
|
+
def move_trajectory(
|
|
1238
|
+
self,
|
|
1239
|
+
points: Iterable[tuple[float, Sequence[float]]],
|
|
1240
|
+
robot: int = 0,
|
|
1241
|
+
loop: bool = False,
|
|
1242
|
+
) -> None:
|
|
1243
|
+
"""Play timestamped joint waypoints on one robot.
|
|
1244
|
+
|
|
1245
|
+
Args:
|
|
1246
|
+
points: ``(t, q)`` pairs. ``t`` is seconds from trajectory start and
|
|
1247
|
+
must increase strictly; ``q`` contains joint positions in radians.
|
|
1248
|
+
robot: Robot index returned by
|
|
1249
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1250
|
+
loop: Repeat from the first point after the last point.
|
|
1251
|
+
|
|
1252
|
+
Returns:
|
|
1253
|
+
Playback is started asynchronously in the browser.
|
|
1254
|
+
|
|
1255
|
+
Raises:
|
|
1256
|
+
TypeError: If a waypoint is not a ``(time, joints)`` pair.
|
|
1257
|
+
ValueError: If a waypoint time or joint value cannot convert to ``float``.
|
|
1258
|
+
RuntimeError: If the client is not connected.
|
|
1259
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1260
|
+
OSError: If the selected transport cannot send the command.
|
|
1261
|
+
|
|
1262
|
+
Notes:
|
|
1263
|
+
The browser interpolates by simulation time. Physics mode servos
|
|
1264
|
+
through the normal controller; kinematics-only mode applies the
|
|
1265
|
+
interpolated target directly. The last point remains held until a new
|
|
1266
|
+
motion, stop, reset, or trajectory-stop command arrives.
|
|
1267
|
+
|
|
1268
|
+
Motion-panel exports store ``t`` in milliseconds. Divide those values
|
|
1269
|
+
by 1000 before passing them here.
|
|
1270
|
+
|
|
1271
|
+
Examples:
|
|
1272
|
+
Play a two-second out-and-back motion::
|
|
1273
|
+
|
|
1274
|
+
home = sim.describe()[0].home
|
|
1275
|
+
bent = [value + 0.2 for value in home]
|
|
1276
|
+
sim.move_trajectory([(0.0, home), (1.0, bent), (2.0, home)])
|
|
1277
|
+
"""
|
|
1278
|
+
self._send(
|
|
1279
|
+
{
|
|
1280
|
+
"type": "joint_trajectory",
|
|
1281
|
+
"points": [{"t": float(t), "q": [float(v) for v in q]} for t, q in points],
|
|
1282
|
+
"robot": robot,
|
|
1283
|
+
"loop": loop,
|
|
1284
|
+
}
|
|
1285
|
+
)
|
|
1286
|
+
|
|
1287
|
+
def stop_trajectory(self, robot: int = 0) -> None:
|
|
1288
|
+
"""Cancel trajectory playback and hold the current joint pose.
|
|
1289
|
+
|
|
1290
|
+
Args:
|
|
1291
|
+
robot: Robot index returned by
|
|
1292
|
+
[`describe`][sim2bot.client.Robot.describe].
|
|
1293
|
+
|
|
1294
|
+
Returns:
|
|
1295
|
+
Playback is cancelled and the current pose is held.
|
|
1296
|
+
|
|
1297
|
+
Raises:
|
|
1298
|
+
RuntimeError: If the client is not connected.
|
|
1299
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1300
|
+
OSError: If the selected transport cannot send the command.
|
|
1301
|
+
"""
|
|
1302
|
+
self._send({"type": "joint_trajectory_stop", "robot": robot})
|
|
1303
|
+
|
|
1304
|
+
# --- Debug markers (RViz-style) --------------------------------------------
|
|
1305
|
+
|
|
1306
|
+
def marker(
|
|
1307
|
+
self,
|
|
1308
|
+
marker_id: str,
|
|
1309
|
+
shape: str,
|
|
1310
|
+
position: Optional[Sequence[float]] = None,
|
|
1311
|
+
orientation: Optional[Sequence[float]] = None,
|
|
1312
|
+
scale: Optional[Union[float, Sequence[float]]] = None,
|
|
1313
|
+
color: Optional[Sequence[float]] = None,
|
|
1314
|
+
points: Optional[Sequence[Sequence[float]]] = None,
|
|
1315
|
+
from_: Optional[Sequence[float]] = None,
|
|
1316
|
+
to: Optional[Sequence[float]] = None,
|
|
1317
|
+
text: Optional[str] = None,
|
|
1318
|
+
) -> None:
|
|
1319
|
+
"""Create or update an RViz-style visual debug marker by ID.
|
|
1320
|
+
|
|
1321
|
+
Args:
|
|
1322
|
+
marker_id: Scene-unique marker ID. Reuse it to update the same marker.
|
|
1323
|
+
shape: ``sphere``, ``box``, ``arrow``, ``line``, ``text``, ``axes``,
|
|
1324
|
+
or ``points``.
|
|
1325
|
+
position: World-frame ``[x, y, z]`` in metres.
|
|
1326
|
+
orientation: World-frame quaternion ``[x, y, z, w]``.
|
|
1327
|
+
scale: Uniform size or ``[x, y, z]`` dimensions in metres.
|
|
1328
|
+
color: RGBA components from 0 to 1.
|
|
1329
|
+
points: World-frame point list for a polyline or point cloud.
|
|
1330
|
+
from_: World-frame arrow start ``[x, y, z]``.
|
|
1331
|
+
to: World-frame arrow end ``[x, y, z]``.
|
|
1332
|
+
text: Label content for a text marker.
|
|
1333
|
+
|
|
1334
|
+
Returns:
|
|
1335
|
+
Reusing ``marker_id`` updates the existing marker.
|
|
1336
|
+
|
|
1337
|
+
Raises:
|
|
1338
|
+
TypeError: If a vector argument is not an iterable of numbers.
|
|
1339
|
+
ValueError: If a vector component cannot be converted to ``float``.
|
|
1340
|
+
RuntimeError: If the client is not connected.
|
|
1341
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1342
|
+
OSError: If the selected transport cannot send the command.
|
|
1343
|
+
|
|
1344
|
+
Notes:
|
|
1345
|
+
Markers are visual-only scene helpers. They do not alter physics and
|
|
1346
|
+
are intentionally excluded from simulated camera feeds.
|
|
1347
|
+
|
|
1348
|
+
Examples:
|
|
1349
|
+
Draw a target position::
|
|
1350
|
+
|
|
1351
|
+
sim.marker(
|
|
1352
|
+
"goal",
|
|
1353
|
+
"sphere",
|
|
1354
|
+
position=[0.45, 0.0, 0.35],
|
|
1355
|
+
scale=0.06,
|
|
1356
|
+
color=[0.36, 0.54, 0.92, 1.0],
|
|
1357
|
+
)
|
|
1358
|
+
"""
|
|
1359
|
+
spec: dict = {"id": marker_id, "shape": shape}
|
|
1360
|
+
if position is not None:
|
|
1361
|
+
spec["position"] = [float(v) for v in position]
|
|
1362
|
+
if orientation is not None:
|
|
1363
|
+
spec["orientation"] = [float(v) for v in orientation]
|
|
1364
|
+
if scale is not None:
|
|
1365
|
+
spec["scale"] = scale if isinstance(scale, (int, float)) else [float(v) for v in scale]
|
|
1366
|
+
if color is not None:
|
|
1367
|
+
spec["color"] = [float(v) for v in color]
|
|
1368
|
+
if points is not None:
|
|
1369
|
+
spec["points"] = [[float(v) for v in p] for p in points]
|
|
1370
|
+
if from_ is not None:
|
|
1371
|
+
spec["from"] = [float(v) for v in from_]
|
|
1372
|
+
if to is not None:
|
|
1373
|
+
spec["to"] = [float(v) for v in to]
|
|
1374
|
+
if text is not None:
|
|
1375
|
+
spec["text"] = str(text)
|
|
1376
|
+
self._send({"type": "marker", "marker": spec})
|
|
1377
|
+
|
|
1378
|
+
def delete_marker(self, marker_id: str) -> None:
|
|
1379
|
+
"""Remove one debug marker.
|
|
1380
|
+
|
|
1381
|
+
Args:
|
|
1382
|
+
marker_id: ID previously supplied to
|
|
1383
|
+
[`marker`][sim2bot.client.Robot.marker].
|
|
1384
|
+
|
|
1385
|
+
Returns:
|
|
1386
|
+
Deleting an unknown marker ID is harmless.
|
|
1387
|
+
|
|
1388
|
+
Raises:
|
|
1389
|
+
RuntimeError: If the client is not connected.
|
|
1390
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1391
|
+
OSError: If the selected transport cannot send the command.
|
|
1392
|
+
"""
|
|
1393
|
+
self._send({"type": "marker_delete", "id": marker_id})
|
|
1394
|
+
|
|
1395
|
+
def clear_markers(self) -> None:
|
|
1396
|
+
"""Remove every SDK-created debug marker from the current scene.
|
|
1397
|
+
|
|
1398
|
+
Returns:
|
|
1399
|
+
Robots, objects, cameras, and room elements are unaffected.
|
|
1400
|
+
|
|
1401
|
+
Raises:
|
|
1402
|
+
RuntimeError: If the client is not connected.
|
|
1403
|
+
ConnectionClosed: If the WebSocket closes while sending the command.
|
|
1404
|
+
OSError: If the selected transport cannot send the command.
|
|
1405
|
+
"""
|
|
1406
|
+
self._send({"type": "marker_clear"})
|
|
1407
|
+
|
|
1408
|
+
# -- cameras -------------------------------------------------------------
|
|
1409
|
+
|
|
1410
|
+
def camera(
|
|
1411
|
+
self,
|
|
1412
|
+
camera_id: str,
|
|
1413
|
+
fps: Optional[int] = None,
|
|
1414
|
+
width: Optional[int] = None,
|
|
1415
|
+
height: Optional[int] = None,
|
|
1416
|
+
quality: Optional[float] = None,
|
|
1417
|
+
codec: Any = None,
|
|
1418
|
+
) -> CameraStream:
|
|
1419
|
+
"""Subscribe to a global scene camera and return a latest-frame stream.
|
|
1420
|
+
|
|
1421
|
+
Args:
|
|
1422
|
+
camera_id: Required ID returned by
|
|
1423
|
+
[`cameras`][sim2bot.client.Robot.cameras], such as
|
|
1424
|
+
``model:0``, ``custom:<id>``, or ``sensor:<id>``.
|
|
1425
|
+
fps: Requested frame rate from 1 to 30. ``None`` inherits GUI/default
|
|
1426
|
+
stream settings.
|
|
1427
|
+
width: Requested width in pixels; currently clamped to 16–1920.
|
|
1428
|
+
height: Requested height in pixels; currently clamped to 16–1080.
|
|
1429
|
+
quality: JPEG quality from 0.1 to 1.0. Ignored for raw frames.
|
|
1430
|
+
codec: ``"jpeg"`` for compact frames or ``"raw"`` for uncompressed
|
|
1431
|
+
RGBA. H.264 is reserved for later.
|
|
1432
|
+
|
|
1433
|
+
Returns:
|
|
1434
|
+
A [`CameraStream`][sim2bot.client.CameraStream]. Close it or use a
|
|
1435
|
+
context manager to stop the subscription.
|
|
1436
|
+
|
|
1437
|
+
Raises:
|
|
1438
|
+
RuntimeError: If the control client is not connected.
|
|
1439
|
+
TimeoutError: If the video WebSocket cannot connect before its timeout.
|
|
1440
|
+
ConnectionClosed: If either WebSocket closes during subscription.
|
|
1441
|
+
OSError: If the video or control endpoint cannot be opened or written.
|
|
1442
|
+
|
|
1443
|
+
Notes:
|
|
1444
|
+
Cameras are global scene resources and do not take ``robot=``. One
|
|
1445
|
+
overhead feed can observe several independently addressed robots.
|
|
1446
|
+
Unspecified stream options inherit the camera's GUI configuration.
|
|
1447
|
+
|
|
1448
|
+
Examples:
|
|
1449
|
+
Read one world-mounted camera frame::
|
|
1450
|
+
|
|
1451
|
+
overhead = next(
|
|
1452
|
+
item for item in sim.cameras()
|
|
1453
|
+
if item["mount"]["kind"] == "world"
|
|
1454
|
+
)
|
|
1455
|
+
with sim.camera(overhead["id"], fps=24, width=640, height=480) as stream:
|
|
1456
|
+
frame = stream.read(timeout=2.0)
|
|
1457
|
+
"""
|
|
1458
|
+
self._ensure_video()
|
|
1459
|
+
stream = CameraStream(self, camera_id)
|
|
1460
|
+
# Send only the params the caller specified, so unset ones fall back to
|
|
1461
|
+
# the per-camera GUI default on the sim side (controller overrides GUI).
|
|
1462
|
+
options: dict = {"type": "camera_subscribe", "camera": camera_id}
|
|
1463
|
+
if fps is not None:
|
|
1464
|
+
options["fps"] = fps
|
|
1465
|
+
if width is not None:
|
|
1466
|
+
options["width"] = width
|
|
1467
|
+
if height is not None:
|
|
1468
|
+
options["height"] = height
|
|
1469
|
+
if quality is not None:
|
|
1470
|
+
options["quality"] = quality
|
|
1471
|
+
if codec is not None:
|
|
1472
|
+
options["codec"] = (
|
|
1473
|
+
_CODEC_BY_NAME.get(codec, codec) if isinstance(codec, str) else codec
|
|
1474
|
+
)
|
|
1475
|
+
with self._cameras_lock:
|
|
1476
|
+
self._cameras[camera_id] = (options, stream)
|
|
1477
|
+
self._send(options) # initial subscribe; renewed by _renew_loop
|
|
1478
|
+
return stream
|
|
1479
|
+
|
|
1480
|
+
def _remove_camera(self, camera_id: str) -> None:
|
|
1481
|
+
self._unsubscribe(camera_id)
|
|
1482
|
+
|
|
1483
|
+
def _unsubscribe(self, camera_id: str) -> None:
|
|
1484
|
+
with self._cameras_lock:
|
|
1485
|
+
self._cameras.pop(camera_id, None)
|
|
1486
|
+
try:
|
|
1487
|
+
self._send({"type": "camera_unsubscribe", "camera": camera_id})
|
|
1488
|
+
except Exception:
|
|
1489
|
+
pass
|
|
1490
|
+
|
|
1491
|
+
def _ensure_video(self) -> None:
|
|
1492
|
+
if self._video_conn is not None:
|
|
1493
|
+
return
|
|
1494
|
+
# max_size=None: video frames can exceed the websockets 1 MB default — a
|
|
1495
|
+
# RAW 640x480 RGBA frame is ~1.2 MB, and larger resolutions more. Without
|
|
1496
|
+
# this the client rejects big frames and the feed silently stops.
|
|
1497
|
+
self._video_conn = ws_connect(
|
|
1498
|
+
self.video_url,
|
|
1499
|
+
open_timeout=self._connect_timeout,
|
|
1500
|
+
max_size=None,
|
|
1501
|
+
additional_headers=self._auth_headers(),
|
|
1502
|
+
)
|
|
1503
|
+
hello = {"type": "hello", "role": "controller-video"}
|
|
1504
|
+
if self._room:
|
|
1505
|
+
hello["room"] = self._room
|
|
1506
|
+
self._video_conn.send(json.dumps(hello))
|
|
1507
|
+
self._spawn(self._video_reader, name="sim2bot-video")
|
|
1508
|
+
|
|
1509
|
+
def _auth_headers(self) -> Optional[dict[str, str]]:
|
|
1510
|
+
if not self._api_key:
|
|
1511
|
+
return None
|
|
1512
|
+
return {"Authorization": f"Bearer {self._api_key}"}
|
|
1513
|
+
|
|
1514
|
+
def _video_reader(self) -> None:
|
|
1515
|
+
conn = self._video_conn
|
|
1516
|
+
if conn is None:
|
|
1517
|
+
return
|
|
1518
|
+
while not self._stop.is_set():
|
|
1519
|
+
try:
|
|
1520
|
+
raw = conn.recv(timeout=1.0)
|
|
1521
|
+
except TimeoutError:
|
|
1522
|
+
continue
|
|
1523
|
+
except ConnectionClosed:
|
|
1524
|
+
break
|
|
1525
|
+
if not isinstance(raw, (bytes, bytearray)):
|
|
1526
|
+
continue
|
|
1527
|
+
frame = _parse_frame(bytes(raw))
|
|
1528
|
+
if frame is None:
|
|
1529
|
+
continue
|
|
1530
|
+
with self._cameras_lock:
|
|
1531
|
+
entry = self._cameras.get(frame.camera_id)
|
|
1532
|
+
if entry is not None:
|
|
1533
|
+
entry[1]._deliver(frame)
|
|
1534
|
+
|
|
1535
|
+
def _renew_loop(self) -> None:
|
|
1536
|
+
while not self._stop.wait(_RENEW_INTERVAL_S):
|
|
1537
|
+
with self._cameras_lock:
|
|
1538
|
+
options = [opts for opts, _ in self._cameras.values()]
|
|
1539
|
+
for opts in options:
|
|
1540
|
+
try:
|
|
1541
|
+
self._send(opts)
|
|
1542
|
+
except Exception:
|
|
1543
|
+
return
|