soarm-ws: A Full-Stack Workspace for SO-ARM100 Manipulator Research

1. IMU sampling

imu_sdk — a background serial thread, decoupled from the control loop

An M5StickC/ESP32 board (MPU6050/6500/6886/9250) streams JSON accelerometer + gyroscope readings over serial at roughly 100 Hz, with gyro bias calibration done on-device at boot. A reader thread parses each line into an ImuData record and pushes it onto a queue; the 50 Hz control loop drains it non-blocking, so a late or dropped IMU frame never stalls a tick — it just falls back to whatever the EKF last held.

imu: ImuData | None = None
try:
    imu = imu_queue.get_nowait()
except queue.Empty:
    pass  # no new sample this tick — EKF simply isn't updated

if imu is not None:
    ekf.step(imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz, dt)

2. Attitude estimation — error-state Kalman filter

m5teleop/imu_ekf.py — ImuEKF

A raw gyro integral drifts, and a raw accelerometer is noisy and only observes gravity's direction — never yaw. The ESKF fixes both: it tracks a nominal state (unit quaternion q̄, gyro bias b̄) with a separate small-angle error state δx = [δθ, δb] ∈ ℝ⁶ and its 6×6 covariance P. Every tick is predict-then-update:

A third update — ZARU (zero angular-rate update) — fires whenever the raw gyro norm drops below a threshold: at rest, ω_meas ≈ bias directly, which is what pins down the yaw-axis bias that gravity alone can never observe (accelerometer updates are blind to rotation about the gravity vector). All six noise/gate constants are tuned offline by tune_ekf.py against logged sessions rather than hand-picked.

def step(self, ax, ay, az, gx_dps, gy_dps, gz_dps, dt):
    omega = np.deg2rad([gx_dps, gy_dps, gz_dps]) - self._bias
    self._predict(omega, dt)
    self._update(np.array([ax, ay, az]))            # skipped under shock/free-fall
    self._update_zaru(np.deg2rad([gx_dps, gy_dps, gz_dps]))
    return self._q.copy()                            # q_imu, [w, x, y, z]

3. Cascade orientation controller

m5teleop/orient_controller.py — OrientationController

Pressing the IMU's button A calls zero_reset(), which memorises the current q_imu_ref and q_ee_ref — the mapping is coordinate-frame agnostic, so whatever way you're holding the IMU at that instant becomes "zero." From then on the target EE orientation is the EE reference rotated by however much the IMU has rotated since reset:

Two proportional loops turn that target into a joint-space-agnostic 6-D twist. The outer loop converts orientation error to an angular-velocity setpoint using a rotation vector (axis × angle) rather than Euler angles — it has no gimbal lock and stays well-defined for any error up to 180°:

The inner loop matters because the outer loop alone only reacts to position error — it has no way to know the arm is already moving toward the target. Estimating the EE's actual angular velocity from consecutive forward-kinematics rotations and feeding that back damps overshoot without needing a second sensor.

if teleop_active and imu is not None:
    q_imu = ekf.quaternion
    q_ee  = _get_ee_quaternion()          # from IK solver's current FK
    twist, orient_err = orient_ctrl.compute_twist(q_imu, q_ee, dt)
else:
    twist = zero_twist                    # teleop off → arm holds position

4. Differential inverse kinematics

m5teleop/ik_solver.py — IKSolver, wrapping pink + pinocchio

The twist is first integrated into a target end-effector pose, then solved as a weighted least-squares QP over joint velocities — pink's standard formulation, with two tasks:

FrameTask pulls the EE toward target_se3 (position cost 1.0, orientation cost 0.5); PostureTask (cost 1e-3) regularises the null space toward a neutral posture — with only 5 revolute joints solving a 6-D pose task, the arm is at its kinematic limit, and this term is what picks one specific solution instead of leaving the system under-constrained. The QP itself is solved with quadprog.

def step(self, twist_world, dt):
    current_ee = self.configuration.get_transform_frame_to_world(self._ee_frame)
    new_translation = current_ee.translation + twist_world[:3] * dt
    dR = pin.AngleAxis(norm(twist_world[3:] * dt), axis).toRotationMatrix()
    self._ee_task.set_target(pin.SE3(dR @ current_ee.rotation, new_translation))

    velocity = pink.solve_ik(self.configuration, [self._ee_task, self._posture_task],
                              dt, solver="quadprog")
    self.configuration.integrate_inplace(velocity, dt)
    return np.array(self.configuration.q)     # joint radians

5. Servo dispatch, logging, and (optional) recording

lerobot SO100Follower · Rerun · soarm_lerobot

The five revolute joints convert radians → degrees and go out over the servo bus through lerobot's SO100Follower; the gripper is driven separately by a toggle bound to button B, not through IK. Every stage's intermediate state — raw IMU, EKF quaternion and bias, orientation error, twist, EE pose, joint configuration — is logged to Rerun on the same tick, so a session can be replayed and inspected stage-by-stage after the fact. Passing --record adds one more branch: frames are handed to soarm_lerobot's TeleopRecorder, which detects episode boundaries and writes LeRobotDataset episodes for later ACT / Diffusion Policy training.

deg_dict = ik.q_to_degrees(q_current)         # 5 joints, radians → degrees
arm.send_joint_degrees(deg_dict)

if viz is not None and imu is not None:
    viz.log_all_with_tracking(imu=imu, twist=twist, q=q_current,
                               ee_translation=ee_pose.translation,
                               ee_rotation=ee_pose.rotation,
                               ekf_euler=ekf.euler_deg, ekf_bias=ekf.bias_dps,
                               orient_err=orient_err, ...)

1. Task & command setup

soarm_mjlab/tasks/reach/reach_env_cfg.py, config/so_arm100/env_cfgs.py

The Reach task's target isn't a hand-picked box — it's derived from the robot itself. A throwaway MuJoCo model is compiled once, 20,000 random joint configurations are sampled within the arm's hard limits, forward kinematics gives the end-effector position for each, and after discarding near-ground samples the 10th–90th percentile box per axis becomes the command-sampling range: a reliably reachable region instead of the full, mostly-unreachable-in-practice workspace envelope. Every episode reset also applies domain randomization — joint positions offset ±0.1 rad from default, zero initial velocity — so the policy never sees the exact same start state twice.

samples = rng.uniform(joint_ranges[:, 0], joint_ranges[:, 1], size=(20_000, ndof))
for i in range(20_000):
    data.qpos[:] = samples[i]
    mujoco.mj_kinematics(model, data)
    positions[i] = data.site_xpos[ee_site_id]

positions = positions[positions[:, 2] > min_ground_clearance]
lo, hi = np.percentile(positions, [10.0, 90.0], axis=0)   # → per-axis command range

2. Manager-based MDP tick

tasks/reach/mdp/{observations,rewards,terminations}.py

Every simulation step, five managers run in order: observations are assembled, the action term applies the policy's output, physics steps, rewards are summed from independent terms, and terminations are checked. Observations come in two flavors — an actor group with injected uniform noise (what the policy trains and infers on, since real sensors are noisy) and a clean critic group (privileged, noise-free — the value function can use ground truth because it never runs at deployment):

The action term is a joint-position offset around each joint's default, and the reward is deliberately the smallest set that produces a non-degenerate policy — orientation error is observed but not yet scored (orientation_weight=0.0, raised only once orientation targets matter):

def distance_to_target(env, command_name, asset_cfg):
    ee_pos_b, _ = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
    position_error = torch.norm(command[:, :3] - ee_pos_b, dim=-1)
    return -position_error          # reward is negative error, orientation_weight=0.0

3. PPO update

tasks/reach/config/so_arm100/rl_cfg.py — RSL-RL

After collecting 24 steps of rollout per parallel environment, RSL-RL computes generalized advantage estimates and updates a 3-layer (128, 128, 64) ELU actor-critic with the standard PPO clipped-surrogate objective, 5 epochs over 4 minibatches per update:

algorithm=RslRlPpoAlgorithmCfg(
    clip_param=0.2, entropy_coef=0.005, gamma=0.99, lam=0.95,
    num_learning_epochs=5, num_mini_batches=4, learning_rate=1.0e-3,
    schedule="adaptive", desired_kl=0.01, max_grad_norm=1.0,
)
# actor/critic: hidden_dims=(128, 128, 64), activation="elu", obs_normalization=True

The learning rate adapts to keep the policy's KL divergence per update near desired_kl rather than using a fixed schedule. CI's own training smoke tests run CPU-only (2 iterations, no GPU in the loop); a real run — thousands of parallel envs, full iteration count — targets a rented GPU instead, since none of this workspace's own dev machines has one. Weights & Biases tracking is on by default (reward curve, all episode termination/reward/metric scalars); see the vast.ai training guide for instance sizing and the full walkthrough.

4. Validation ladder — before any checkpoint touches hardware

test pyramid, shaped around one constraint: real physics and GPU training can't run in CI

A checkpoint is promoted to real hardware only after clearing a numeric bar decided before the run (e.g. ≥90% task_success over 100 held-out seeds) — set in advance so the goalposts can't move to match whatever the run happened to produce.

5. Sim2real deployment planned

deploy/reach_policy_runner.py — not yet written

The deployment script is designed to be trivial by construction: it loads a promoted checkpoint and calls the same RobotInterface protocol regardless of whether the concrete implementation underneath is a MuJoCo SimRobotInterface or soarm_sdk.ServoRobot talking to the real bus — one code path, no train/deploy consistency problem to solve because there's only one implementation. Unlike a legged robot's balance controller, a 6-DOF arm's ~50 Hz loop already runs comfortably in Python, so there's deliberately no C++ deployment stack to maintain in parallel. Promotion already has a publish step ready ahead of this script: scripts/push_to_hub.py pushes a promoted checkpoint (ONNX export, resolved configs, a generated model card with training provenance) to a Hugging Face Hub model repo, so this deployment script — or anyone else — can load one without needing access to the original training run.

1. Goal specification

planned — reuses m5teleop's existing URDF/pinocchio model

A goal is either a target joint configuration directly, or a Cartesian end-effector pose resolved to one via roboplan's IK solver — the same so100.urdf pinocchio model m5teleop/ik_solver.py already loads for real-time differential IK, just consumed by a different, offline solve rather than a 50 Hz velocity-level QP.

2. Collision-free path search

planned — roboplan's RRT planner, joint space ℝ⁵

An RRT grows a tree from the start configuration, at each step sampling a random joint configuration, steering the nearest tree node toward it by a bounded step, and keeping the new node only if the edge is collision-free against roboplan's pinocchio-backed geometry model (self-collision and environment). The result is a valid but geometrically jagged sequence of waypoints — path existence, not path quality:

3. Trajectory timing

planned — roboplan wraps TOPP-RA

The RRT path is a geometric curve with no notion of time. TOPP-RA parameterizes it by arc length s ∈ [0, 1] and solves for the fastest s(t) that keeps every joint's velocity and acceleration within limits along the whole path — a minimum-time trajectory on a fixed geometric path, not a hand-tuned trapezoidal velocity profile:

4. Joint-space tracking

planned — simple position/velocity control, no task-space feedback

Execution is deliberately the least sophisticated controller that can follow a pre-computed trajectory — the same "smallest thing that works" philosophy already applied to the RL reward: stream q_des(t) to each STS3215 servo's own onboard position PID (the same mechanism soarm_sdk's dashboard already uses for homing), optionally adding a joint-space proportional term with velocity feedforward where the bus protocol supports it:

No Jacobian, no orientation controller, no learned dynamics model — the trajectory already encodes everything about the motion; the controller's only job is to not fall too far behind it.