SpaceMouse API¤
このページでは、SpaceMouse による SO-101 制御に関連する API を説明します。
概要¤
SpaceMouse 統合は以下のコンポーネントで構成されます:
- SpaceMouseReader:
pyspacemouseのスレッドベースラッパー。デバイスをバックグラウンドでポーリングし、最新の 6-DOF 状態を提供 - SpaceMouseConfig: 速度スケーリング、デッドゾーン等の設定
- So101SpaceMouseController: SpaceMouse → EE デルタ → IK → Follower の制御パイプライン
- So101SpaceMouseExpHandler: データ収集用の対話的ハンドラ
graph TD
A[SpaceMouseReader] -->|SpaceMouseState| B[So101SpaceMouseController]
C[SpaceMouseConfig] --> B
D[So101Robot] --> B
B -->|So101Obs| E[So101SpaceMouseExpHandler]
E -->|HDF5/GIF| F[So101SaveWorker]
入力デバイス¤
SpaceMouseState¤
SpaceMouseState
dataclass
¤
SpaceMouseState(x: float = 0.0, y: float = 0.0, z: float = 0.0, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0, buttons: list[bool] = (lambda: [False, False])(), timestamp: float = 0.0)
Snapshot of the SpaceMouse 6-DOF axes and button states.
All axis values are normalised to [-1.0, 1.0].
SpaceMouseReader¤
SpaceMouseReader
¤
Thread-based wrapper around pyspacemouse.
A background daemon thread polls pyspacemouse.read() at high frequency
and stores the latest state behind a lock so that callers can retrieve it
without blocking.
Usage::
reader = SpaceMouseReader()
reader.start()
state = reader.get_state()
reader.stop()
| METHOD | DESCRIPTION |
|---|---|
start |
Open the SpaceMouse device and start the polling thread. |
stop |
Stop the polling thread and close the device. |
get_state |
Return the latest SpaceMouse state (non-blocking). |
| ATTRIBUTE | DESCRIPTION |
|---|---|
is_running |
TYPE:
|
Source code in src/robopy/input/spacemouse.py
start
¤
Open the SpaceMouse device and start the polling thread.
Source code in src/robopy/input/spacemouse.py
stop
¤
Stop the polling thread and close the device.
Source code in src/robopy/input/spacemouse.py
get_state
¤
get_state() -> SpaceMouseState
Return the latest SpaceMouse state (non-blocking).
Source code in src/robopy/input/spacemouse.py
設定¤
SpaceMouseConfig¤
SpaceMouseConfig
dataclass
¤
SpaceMouseConfig(linear_speed: float = 0.1, angular_speed: float = 0.5, gripper_speed: float = 50.0, deadzone: float = 0.05, control_hz: int = 50, input_smoothing: float = 0.5, max_joint_delta_deg: float = 5.0)
Configuration for SpaceMouse input device.
| ATTRIBUTE | DESCRIPTION |
|---|---|
linear_speed |
Maximum linear velocity in m/s when the SpaceMouse axis is at full deflection (1.0).
TYPE:
|
angular_speed |
Maximum angular velocity in rad/s when the SpaceMouse axis is at full deflection (1.0).
TYPE:
|
gripper_speed |
Gripper movement speed in degrees/s per button press.
TYPE:
|
deadzone |
Axes with absolute values below this threshold are zeroed.
TYPE:
|
control_hz |
Control loop frequency in Hz.
TYPE:
|
input_smoothing |
Exponential moving average factor for SpaceMouse axes. 0.0 = no smoothing (raw input), 1.0 = maximum smoothing. Higher values reduce jitter but add latency.
TYPE:
|
max_joint_delta_deg |
Maximum allowed joint angle change per control step in degrees. Prevents vibration from large IK solution jumps.
TYPE:
|
コントローラー¤
So101SpaceMouseController¤
So101SpaceMouseController
¤
So101SpaceMouseController(robot: So101Robot, spacemouse_config: SpaceMouseConfig | None = None)
Control an SO-101 follower arm via a 3Dconnexion SpaceMouse.
The SpaceMouse 6-DOF input (x, y, z, roll, pitch, yaw — all normalised
to [-1, 1]) is mapped to incremental end-effector velocity commands.
At each control step the current EE pose is updated by the velocity
delta, then Inverse Kinematics converts the target pose to joint angles
that are sent to the follower arm.
The controller operates without a physical leader arm — the SpaceMouse replaces it. Gripper open/close is driven by the left/right buttons.
| PARAMETER | DESCRIPTION |
|---|---|
robot
|
A configured :class:
TYPE:
|
spacemouse_config
|
Optional tuning parameters. Defaults are reasonable for desktop-scale manipulation.
TYPE:
|
| METHOD | DESCRIPTION |
|---|---|
connect |
Connect the robot and start the SpaceMouse reader. |
disconnect |
Stop the SpaceMouse reader and disconnect the robot. |
teleoperation |
Run real-time SpaceMouse → EE-space teleoperation. |
record_parallel |
Record a SpaceMouse-driven trajectory with camera observations. |
| ATTRIBUTE | DESCRIPTION |
|---|---|
robot |
TYPE:
|
is_connected |
TYPE:
|
Source code in src/robopy/robots/so101/so101_spacemouse.py
connect
¤
Connect the robot and start the SpaceMouse reader.
disconnect
¤
Stop the SpaceMouse reader and disconnect the robot.
teleoperation
¤
Run real-time SpaceMouse → EE-space teleoperation.
The control loop reads the SpaceMouse at control_hz, computes the incremental EE delta, runs IK and sends joint angles to the follower.
| PARAMETER | DESCRIPTION |
|---|---|
max_seconds
|
If given, stop after this many seconds. Otherwise
the loop runs until interrupted (
TYPE:
|
Source code in src/robopy/robots/so101/so101_spacemouse.py
record_parallel
¤
record_parallel(max_frame: int, fps: int = 20, control_hz: int | None = None, is_async: bool = True) -> So101Obs
Record a SpaceMouse-driven trajectory with camera observations.
Architecture mirrors :meth:So101Robot.record_parallel — a fast
control thread drives the arm while the main thread captures camera
frames at the requested fps.
The returned So101Obs.arms.leader contains the commanded joint
angles (IK output), so the trajectory can be replayed verbatim with
So101Robot.send(). So101Obs.arms.follower contains the
actual motor readings.
| PARAMETER | DESCRIPTION |
|---|---|
max_frame
|
Number of sensor frames to record.
TYPE:
|
fps
|
Camera / observation capture rate.
TYPE:
|
control_hz
|
SpaceMouse control loop rate (defaults to config).
TYPE:
|
is_async
|
Use asynchronous camera reads when available.
TYPE:
|
| RETURNS | DESCRIPTION |
|---|---|
So101Obs
|
|
Source code in src/robopy/robots/so101/so101_spacemouse.py
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 | |
実験ハンドラ¤
So101SpaceMouseExpHandler¤
So101SpaceMouseExpHandler
¤
So101SpaceMouseExpHandler(so101_config: So101Config, metadata_config: MetaDataConfig, spacemouse_config: SpaceMouseConfig | None = None, fps: int = 20)
Bases: ExpHandler[So101Obs, So101Robot, So101Config, So101SaveWorker]
Experiment handler using SpaceMouse for SO-101 data collection.
This handler replaces the leader arm with a SpaceMouse device for
teleoperation and data recording. It follows the standard
:class:ExpHandler interface so that the interactive record_save()
workflow (warmup → record → save) works identically.
| METHOD | DESCRIPTION |
|---|---|
record |
|
send |
|
record_save |
Interactive record-and-save loop using SpaceMouse. |
close |
Disconnect SpaceMouse + robot and shut down the save worker. |
| ATTRIBUTE | DESCRIPTION |
|---|---|
controller |
|
Source code in src/robopy/utils/exp_interface/so101_spacemouse_exp_handler.py
record
¤
Source code in src/robopy/utils/exp_interface/so101_spacemouse_exp_handler.py
send
¤
record_save
¤
record_save(max_frames: int, save_path: str, if_async: bool = True, save_gif: bool = True, warmup_time: int = 5) -> None
Interactive record-and-save loop using SpaceMouse.
Overrides the base class to use SpaceMouse teleoperation for the warmup phase (instead of the leader-arm-based teleoperation).
Source code in src/robopy/utils/exp_interface/so101_spacemouse_exp_handler.py
close
¤
Disconnect SpaceMouse + robot and shut down the save worker.
Source code in src/robopy/utils/exp_interface/so101_spacemouse_exp_handler.py
使用例¤
基本的なテレオペレーション¤
from robopy.config.robot_config.so101_config import So101Config
from robopy.robots.so101.so101_robot import So101Robot
from robopy.robots.so101.so101_spacemouse import So101SpaceMouseController
config = So101Config(
follower_port="/dev/ttyACM0",
calibration_path="calibration/so101_calib.json",
)
robot = So101Robot(cfg=config)
controller = So101SpaceMouseController(robot)
controller.connect()
# 30秒間操作
controller.teleoperation(max_seconds=30)
controller.disconnect()
カスタム設定でのデータ収集¤
from robopy.config.input_config.spacemouse_config import SpaceMouseConfig
sm_config = SpaceMouseConfig(
linear_speed=0.05, # 低速で精密操作
angular_speed=0.3,
deadzone=0.08, # 大きめのデッドゾーン
control_hz=100, # 高速な制御ループ
)
controller = So101SpaceMouseController(robot, spacemouse_config=sm_config)
controller.connect()
obs = controller.record_parallel(max_frame=1000, fps=30)
EE デルタの計算式¤
各制御ステップで、SpaceMouse の軸値から EE デルタを計算します:
$$ \Delta x = \text{axis}_x \times \text{linear_speed} \times dt $$
$$ \Delta \text{pitch} = \text{axis}_\text{pitch} \times \text{angular_speed} \times dt $$
ここで $dt = 1 / \text{control_hz}$ です。デッドゾーン内の軸値($|\text{axis}| < \text{deadzone}$)はゼロとして扱われます。