Making it move · Chapter 13 · Time: 3 hours · Level: Advanced · Status: Partly test-built
The six arm servos driven safely by the base driver - pulses and radians, the mapped limits and named poses, smooth quintic moves that refuse anything outside the limits, a collision check before every move, tuck and auto-tuck - plus the parked MoveIt setup.
The arm is five joints and a claw built from bus servos on the controller board's servo bus. On this robot its main
job is to carry the depth camera: it is the robot's head. It can hit the robot's own body, the LiDAR tower and you,
and it drops when its servos lose power. So nothing talks to the servos except the base driver, every goal outside
the mapped limits is refused, every move is checked against a box model of the robot first, and when the arm has had
nothing to do for two minutes it folds itself onto its own frame and switches its servos off. Every rule in this
chapter comes from a test or an incident on this robot, between 2026-09-27 and 2026-10-05.
New idea: bus servos
A hobby servo gets a position from a PWM pulse on its own wire. A bus servo shares one serial line with the
others; each has an ID and answers only frames addressed to it. You send "go to position P in T milliseconds" and
the servo's own controller drives there and holds. You can also read back its position, supply voltage,
temperature and whether its motor is powered ("torque on"). These are HX-12H servos (inventory 2026-08-15); the
STM32 board relays our frames to the servo bus as function 5 of the protocol from chapter 10.
New idea: joint space, pulses and radians
The arm's pose is a list of joint angles, one per joint: its position in joint space. Each servo works in
pulses from 0 to 1000, with 500 in the middle. ROS works in radians. The conversion is the factory's
(model/robot_model.jsonpulse_to_rad):rad = (500 - pulse) * 0.0041887902047863905One pulse is 0.24 degrees; 1000 pulses span 240 degrees. The sign is "flipped": more pulses = a negative angle.
Code that talks to servos uses pulses; code that talks to ROS (/joint_states, MoveIt, JointJog) uses radians.
| Servo ID | Joint | URDF joint | + pulses means | Stored limit in the servo (read with 0x32) |
|---|---|---|---|---|
| 1 | base (pan) | joint1 |
clockwise seen from above (owner: "I think") | 10-990 |
| 2 | shoulder | joint2 |
back | 80-960 |
| 3 | elbow | joint3 |
- | 10-990 |
| 4 | wrist (tilt) | joint4 |
wrist forward (model) | 0-1000 |
| 5 | claw rotate | joint5 |
- | 10-990 |
| 10 | claw | not in the URDF | close | 110-670 |
The stored limits live in each servo's EEPROM and silently cap motion: servo 2 stops at 960 whatever you send. This
explains every "960/961" stop in the record (docs/lessons.md: read them before concluding a joint is blocked by
geometry). A read on 2026-09-27 found all six servos answering at 12.5-12.7 V and 30-42 degrees C.
No EEPROM writes
Rule 6 of the arm design (docs/motion_safety.md, 2026-09-27): no writes to servo EEPROM - no ID, offset, PID or
baud changes. The factory node rewrote baud rate and PID at every start; this driver never does. The commands
0x12 (ID) and 0x22 (offset) are in the reference table below for reading only.
All servo traffic is function 5 of the board protocol (AA 55 | func | len | payload | CRC-8/MAXIM, chapter 10).
The first payload byte is the servo command.
| Command | Payload | Meaning | Used by the driver |
|---|---|---|---|
| 0x01 move | <BHB (0x01, duration ms u16, n), then n x <BH (id, pulse u16) |
go to these pulses in this time | yes |
| 0x03 stop/hold | [0x03, n, ids...] |
stop where you are and hold | yes (every arm e-stop) |
| 0x0C torque off | [0x0C, id] |
motor off, the joint goes limp | yes (relax, tuck) |
| 0x05 read position | [0x05, id] -> reply <BBbh |
int16, can be negative after a sag | yes |
| 0x07 read voltage | [0x07, id] -> reply <BBbH |
supply in mV | yes |
| 0x09 read temperature | [0x09, id] -> reply <BBbB |
degrees C | yes |
| 0x0D read torque state | [0x0D, id] -> reply <BBbb |
1 on, 0 off | yes |
| 0x0B torque on | [0x0B, id] |
documented | no: it did not re-enable servo 1; a move command does |
| 0x32 stored limits, 0x12 ID, 0x22 offset | - | EEPROM values | read for diagnosis only |
A reply payload is id u8, cmd u8, ok i8, value; ok is 0 when the read worked. Some frames as bytes, computed with
the repo's encode_frame for this guide:
Board bytes:
aa 55 05 07 01 e8 03 01 01 f4 01 f5 move: 1000 ms, 1 servo, id 1 -> pulse 500 (0x01f4)
aa 55 05 08 03 06 01 02 03 04 05 0a e8 hold: 6 servos, ids 1 2 3 4 5 10
aa 55 05 02 05 01 6f read position of servo 1
aa 55 05 02 0d 01 19 read torque state of servo 1
aa 55 05 02 0c 01 dd torque off servo 1
You will not type these frames. The base driver builds them; you call its services. Sending raw servo frames with
the driver stopped while the arm holds a pose is how the owner's "you're being reckless" lesson was earned
(docs/lessons.md, 2026-09-27).
ros2/rosorin_base/rosorin_base/arm.py holds everything about arm motion that does not need ROS or the serial port:
the trajectory shape, the limit check, the servo frames and the unit conversion. That makes all of it testable on any
computer. Build it in pieces in ~/ros2_ws/src/rosorin_base/rosorin_base/arm.py.
New idea: trajectories and minimum jerk
A trajectory is a position as a function of time. Sending a servo straight to its goal makes it start and
stop with a jolt. A minimum-jerk (quintic) profile moves along s(u) = 10u^3 - 15u^4 + 6u^5 for u from 0 to 1:
speed and acceleration are zero at both ends, so the move starts and stops softly. Its peak speed is 1.875 x
distance / duration, which gives the duration for a speed cap: T = 1.875 x distance / max_speed. The driver
samples the curve every 40 ms (25 Hz) and sends each sample as a 40 ms move frame.
First the header, the profile and the duration rule:
On the robot:
"""Pure arm motion logic (no ROS, no I/O) - docs/motion_safety.md, Arm.
Minimum-jerk (quintic) profile: s(u) = 10u^3 - 15u^4 + 6u^5, u in [0, 1]; velocity and
acceleration are zero at both ends (smooth start/stop). Peak speed = 1.875 * distance / T.
"""
import struct
from rosorin_base.rrc_protocol import encode_frame
FUNC_BUS_SERVO = 5
QUINTIC_PEAK = 1.875
def smooth(u):
u = min(1.0, max(0.0, u))
return u * u * u * (10 - 15 * u + 6 * u * u)
def duration_for(dist, max_speed, min_t=0.2):
"""Trajectory time so the quintic's peak speed stays <= max_speed (pulses/s)."""
return max(min_t, QUINTIC_PEAK * abs(dist) / max_speed)
encode_frame is the frame builder from chapter 10's rrc_protocol.py. The 0.2 s floor keeps tiny moves from
becoming a single jump.
A Trajectory moves several joints together: the slowest joint sets the duration, and every joint follows the same
curve, so they all arrive at once:
On the robot:
class Trajectory:
def __init__(self, start: dict, goal: dict, max_speed: float, t0: float):
self.start, self.goal, self.t0 = dict(start), dict(goal), t0
self.T = max(duration_for(goal[k] - start[k], max_speed) for k in goal)
def at(self, t):
s = smooth((t - self.t0) / self.T)
return {k: int(round(self.start[k] + (self.goal[k] - self.start[k]) * s)) for k in self.goal}
def done(self, t):
return t - self.t0 >= self.T
Example (computed with this code): servo 1 from 500 to 700 at the robot's cap of 250 pulses/s takes 1.5 s, and the
40 ms samples run 500, 500, 500, 501, 502, 504, 506, 510, 514, 519 ... 692, 695, 697, 699, 699, 700, 700, 700: slow
at both ends, fastest in the middle.
New idea: refuse, do not clamp
When a goal is outside a joint's limits, the driver could move to the nearest limit instead ("clamp"). It does
not: the caller asked for something wrong, and a silent substitute hides the mistake and moves the arm somewhere
nobody asked for. The goal is refused with a reason, and nothing moves.
On the robot:
def check_goal(goal: dict, limits: dict):
"""Return None if every joint goal is inside its [lo, hi] limit, else a reason string."""
for k, v in goal.items():
if k not in limits:
return f'servo {k} has no limits configured'
lo, hi = limits[k]
if not lo <= v <= hi:
return f'servo {k} goal {v} outside limits [{lo}, {hi}]'
return None
The next two functions exist because of the claw incident of 2026-10-05 (end of this chapter): one servo that stops
answering must not stop the whole arm. A silent optional servo (the claw) is carried at its last known pulse; a
silent joint servo stays unknown, and the callers refuse:
On the robot:
def fill_optional(pose: dict, last: dict, optional=()):
"""A read pose {id: pulse or None} with every silent OPTIONAL servo carried at its last known pulse.
Returns (pose, silent ids). A silent servo that is not optional stays None: its position is unknown."""
silent = [sid for sid, v in pose.items() if v is None]
out = dict(pose)
for sid in silent:
if sid in optional and last.get(sid) is not None:
out[sid] = int(round(last[sid]))
return out, silent
def torque_all(torque: dict, want: int, optional=()):
"""Every servo reports torque state `want`; a silent optional servo does not count."""
return all(v == want for sid, v in torque.items() if not (v is None and sid in optional))
The two frames the driver writes most, move and hold:
On the robot:
def move_frame(targets: dict, duration_ms: int):
"""func 5 sub 0x01: duration u16, n, (id u8, pulse u16) x n."""
p = struct.pack('<BHB', 0x01, int(duration_ms), len(targets))
for sid, pulse in sorted(targets.items()):
p += struct.pack('<BH', int(sid), int(pulse))
return encode_frame(FUNC_BUS_SERVO, p)
def stop_frame(ids):
"""func 5 sub 0x03: n, ids - servos hold where they are."""
ids = sorted(ids)
return encode_frame(FUNC_BUS_SERVO, bytes([0x03, len(ids)] + ids))
The unit conversion between servo pulses and URDF joint angles. pulses_to_joints feeds /joint_states
(chapter 12); joints_to_pulses turns MoveIt's radians back into pulses:
On the robot:
RAD_PER_PULSE = 0.0041887902047863905 # model/robot_model.json pulse_to_rad (factory, flipped)
JOINT_IDS = (1, 2, 3, 4, 5)
def pulses_to_joints(pulses: dict):
"""{servo id: pulse} -> (['joint1'..'joint5'], [rad]) for URDF joints; None if any joint unknown."""
if any(pulses.get(sid) is None for sid in JOINT_IDS):
return None
return [f'joint{sid}' for sid in JOINT_IDS], [(500 - pulses[sid]) * RAD_PER_PULSE for sid in JOINT_IDS]
def joints_to_pulses(names, positions):
"""Inverse of pulses_to_joints: 'jointN' radians -> {N: pulse}."""
out = {}
for n, rad in zip(names, positions):
if not n.startswith('joint') or not n[5:].isdigit():
raise ValueError(f'unknown joint {n}')
out[int(n[5:])] = int(round(500 - rad / RAD_PER_PULSE))
return out
Last, the trajectory type for MoveIt (Step 7). MoveIt sends a list of timed waypoints that are already smooth. The
driver plays them back linearly between waypoints. If any segment is faster than the speed cap, the whole timeline is
stretched: slower, never refused for speed:
On the robot:
class WaypointTrajectory:
"""Timed waypoints (MoveIt FollowJointTrajectory, already smooth + time-parameterized) streamed like Trajectory:
linear in pulses between waypoints. `start` is the current pose (time 0). If any segment would exceed max_speed
(pulses/s) the WHOLE timeline is stretched - slower, never rejected for speed."""
def __init__(self, start: dict, points: list, max_speed: float, t0: float):
# points: [(t_from_start_s, {sid: pulse})], t strictly increasing, t > 0
self.t0 = t0
self.pts = [(0.0, {k: start[k] for k in points[0][1]})] + [(t, dict(p)) for t, p in points]
scale = 1.0
for (ta, a), (tb, b) in zip(self.pts, self.pts[1:]):
dt = max(tb - ta, 1e-3)
v = max(abs(b[k] - a[k]) for k in b) / dt
scale = max(scale, v / max_speed)
self.scale = scale
self.pts = [(t * scale, p) for t, p in self.pts]
self.T = self.pts[-1][0]
self.goal = self.pts[-1][1]
def at(self, t):
s = t - self.t0
if s >= self.T:
return dict(self.goal)
for (ta, a), (tb, b) in zip(self.pts, self.pts[1:]):
if s <= tb:
f = (s - ta) / max(tb - ta, 1e-6)
return {k: int(round(a[k] + (b[k] - a[k]) * f)) for k in b}
return dict(self.goal)
def done(self, t):
return t - self.t0 >= self.T
def samples(self):
"""Every waypoint plus each segment midpoint (for the collision veto)."""
out = [p for _, p in self.pts]
for (_, a), (_, b) in zip(self.pts, self.pts[1:]):
out.append({k: int(round((a[k] + b[k]) / 2)) for k in b})
return out
The complete file (136 lines, ros2/rosorin_base/rosorin_base/arm.py):
On the robot:
"""Pure arm motion logic (no ROS, no I/O) - docs/motion_safety.md, Arm.
Minimum-jerk (quintic) profile: s(u) = 10u^3 - 15u^4 + 6u^5, u in [0, 1]; velocity and
acceleration are zero at both ends (smooth start/stop). Peak speed = 1.875 * distance / T.
"""
import struct
from rosorin_base.rrc_protocol import encode_frame
FUNC_BUS_SERVO = 5
QUINTIC_PEAK = 1.875
def smooth(u):
u = min(1.0, max(0.0, u))
return u * u * u * (10 - 15 * u + 6 * u * u)
def duration_for(dist, max_speed, min_t=0.2):
"""Trajectory time so the quintic's peak speed stays <= max_speed (pulses/s)."""
return max(min_t, QUINTIC_PEAK * abs(dist) / max_speed)
class Trajectory:
def __init__(self, start: dict, goal: dict, max_speed: float, t0: float):
self.start, self.goal, self.t0 = dict(start), dict(goal), t0
self.T = max(duration_for(goal[k] - start[k], max_speed) for k in goal)
def at(self, t):
s = smooth((t - self.t0) / self.T)
return {k: int(round(self.start[k] + (self.goal[k] - self.start[k]) * s)) for k in self.goal}
def done(self, t):
return t - self.t0 >= self.T
def check_goal(goal: dict, limits: dict):
"""Return None if every joint goal is inside its [lo, hi] limit, else a reason string."""
for k, v in goal.items():
if k not in limits:
return f'servo {k} has no limits configured'
lo, hi = limits[k]
if not lo <= v <= hi:
return f'servo {k} goal {v} outside limits [{lo}, {hi}]'
return None
def fill_optional(pose: dict, last: dict, optional=()):
"""A read pose {id: pulse or None} with every silent OPTIONAL servo carried at its last known pulse.
Returns (pose, silent ids). A silent servo that is not optional stays None: its position is unknown."""
silent = [sid for sid, v in pose.items() if v is None]
out = dict(pose)
for sid in silent:
if sid in optional and last.get(sid) is not None:
out[sid] = int(round(last[sid]))
return out, silent
def torque_all(torque: dict, want: int, optional=()):
"""Every servo reports torque state `want`; a silent optional servo does not count."""
return all(v == want for sid, v in torque.items() if not (v is None and sid in optional))
def move_frame(targets: dict, duration_ms: int):
"""func 5 sub 0x01: duration u16, n, (id u8, pulse u16) x n."""
p = struct.pack('<BHB', 0x01, int(duration_ms), len(targets))
for sid, pulse in sorted(targets.items()):
p += struct.pack('<BH', int(sid), int(pulse))
return encode_frame(FUNC_BUS_SERVO, p)
def stop_frame(ids):
"""func 5 sub 0x03: n, ids - servos hold where they are."""
ids = sorted(ids)
return encode_frame(FUNC_BUS_SERVO, bytes([0x03, len(ids)] + ids))
RAD_PER_PULSE = 0.0041887902047863905 # model/robot_model.json pulse_to_rad (factory, flipped)
JOINT_IDS = (1, 2, 3, 4, 5)
def pulses_to_joints(pulses: dict):
"""{servo id: pulse} -> (['joint1'..'joint5'], [rad]) for URDF joints; None if any joint unknown."""
if any(pulses.get(sid) is None for sid in JOINT_IDS):
return None
return [f'joint{sid}' for sid in JOINT_IDS], [(500 - pulses[sid]) * RAD_PER_PULSE for sid in JOINT_IDS]
def joints_to_pulses(names, positions):
"""Inverse of pulses_to_joints: 'jointN' radians -> {N: pulse}."""
out = {}
for n, rad in zip(names, positions):
if not n.startswith('joint') or not n[5:].isdigit():
raise ValueError(f'unknown joint {n}')
out[int(n[5:])] = int(round(500 - rad / RAD_PER_PULSE))
return out
class WaypointTrajectory:
"""Timed waypoints (MoveIt FollowJointTrajectory, already smooth + time-parameterized) streamed like Trajectory:
linear in pulses between waypoints. `start` is the current pose (time 0). If any segment would exceed max_speed
(pulses/s) the WHOLE timeline is stretched - slower, never rejected for speed."""
def __init__(self, start: dict, points: list, max_speed: float, t0: float):
# points: [(t_from_start_s, {sid: pulse})], t strictly increasing, t > 0
self.t0 = t0
self.pts = [(0.0, {k: start[k] for k in points[0][1]})] + [(t, dict(p)) for t, p in points]
scale = 1.0
for (ta, a), (tb, b) in zip(self.pts, self.pts[1:]):
dt = max(tb - ta, 1e-3)
v = max(abs(b[k] - a[k]) for k in b) / dt
scale = max(scale, v / max_speed)
self.scale = scale
self.pts = [(t * scale, p) for t, p in self.pts]
self.T = self.pts[-1][0]
self.goal = self.pts[-1][1]
def at(self, t):
s = t - self.t0
if s >= self.T:
return dict(self.goal)
for (ta, a), (tb, b) in zip(self.pts, self.pts[1:]):
if s <= tb:
f = (s - ta) / max(tb - ta, 1e-6)
return {k: int(round(a[k] + (b[k] - a[k]) * f)) for k in b}
return dict(self.goal)
def done(self, t):
return t - self.t0 >= self.T
def samples(self):
"""Every waypoint plus each segment midpoint (for the collision veto)."""
out = [p for _, p in self.pts]
for (_, a), (_, b) in zip(self.pts, self.pts[1:]):
out.append({k: int(round((a[k] + b[k]) / 2)) for k in b})
return out
Copy test/test_arm.py from the repo into ~/ros2_ws/src/rosorin_base/test/. Its 15 tests check the profile ends
(smooth(0) == 0, smooth(1) == 1), that the first and last 5 % of a move cover under 1 % of the distance, that the
peak speed stays under the cap even after rounding to whole pulses, that limits refuse instead of clamping, that the
frames parse with chapter 10's parser, that 375 pulses are 90 degrees, and the claw cases. colcon test finds no
tests in this package (docs/status.md: they are not registered), so run pytest directly:
On the robot:
cd ~/ros2_ws/src/rosorin_base
python3 -m pytest -q test/test_arm.py
Check
The last line reads15 passed in ...s. All 15 pass against the repo code (checked for this guide on
2026-10-07). On 2026-09-28 the wholetest/folder of that day gave24 passed in 0.15son the robot.
If it fails
ModuleNotFoundError: rosorin_base.rrc_protocol: run pytest from the package folder
(~/ros2_ws/src/rosorin_base), whererosorin_base/is a subfolder, and make sure chapter 10's
rrc_protocol.pyis there.test_frames_parse_with_our_parserfails: compare yourmove_framewith the listing. The test also checks
the exact bytes[0x01, 40, 0, 1, 1]+ pulse 520 little-endian.
ros2/rosorin_base/config/arm_limits.yaml is a ROS parameter file for the node rosorin_board_driver. The launch
file passes it to board_driver (chapter 11, base.launch.py line 26). It holds the wheel speed limit, the arm's
speed caps, the limits per servo and four named poses. The complete file:
On the robot:
# Arm servo limits (pulses) and speed cap. Mapped 2026-09-27 by gamepad-bounded sweeps (docs/motion_safety.md).
# Per-joint limits are a union envelope; combinations are pose-dependent -> model path check + slow speed.
# Previously: ±50 around the rest pose read 2026-09-27
# (1=500, 2=724, 3=46, 4=149, 5=501, 10=499). Widen only after supervised jogging (motion_safety.md).
rosorin_board_driver:
ros__parameters:
arm_max_speed: 250.0 # owner 2026-09-29: 50 -> 62.5 -> 125 -> 250 (ceiling 300); quintic: accel scales with speed^2
arm_jog_max_speed: 300.0 # owner 2026-10-01: head (JointJog) faster = the 300 ceiling (72 deg/s); goals stay 250
max_linear: 0.35 # wheels (board_driver motion gate; default 0.2): owner 2026-10-01 faster, accel unchanged 0.5 m/s^2
arm_limit_1: [34, 966] # swept 2026-09-27: reach 14..986 both ways, unobstructed
arm_limit_2: [95, 941] # union: from rest back 961 free, fwd owner stop 562 (joint 3 in the way); from straight-up back stop 927, fwd stop 75
arm_limit_3: [40, 881] # straight-up: owner stops 66 / 901; lower kept at 40 so folded rest (44) stays reachable
arm_limit_4: [121, 935] # straight-up: owner stops 101 / 955
arm_limit_5: [31, 969] # rotates freely 11..989
arm_limit_10: [131, 631] # claw: self-stops fully open 111 / fully closed 651 (margin keeps it off its own fingers)
# default pose (owner-designed 2026-09-27): S-coil, wrist camera ~11 deg below level, balanced over
# the base (model). Used by ~/arm/recover. Previous rest: 500/724/46/149/501/499 (folded)
# 2026-10-05 owner placed it by gamepad: "if you come out a little more past rest, the claw rotates freely" (in the
# S-coil the claw collides with the joint-2 servo). Camera 5 cm higher (0.38 m), level +5 deg at wrist 161; it can
# look down to -4 deg only (wrist limit 121), up as before. Read back: 500/776/211/161/492/499.
arm_rest_1: 500
arm_rest_2: 776
arm_rest_3: 211
arm_rest_4: 161
arm_rest_5: 500
arm_rest_10: 499
# fold pose = the S-coil that was the rest pose until 2026-10-05: the step between rest and tuck, both ways
# (the path tuck <-> S-coil is the one run daily since 09-27; wrist roll stays centred through it)
arm_fold_1: 500
arm_fold_2: 860
arm_fold_3: 45
arm_fold_4: 178
arm_fold_5: 500
arm_fold_10: 499
arm_recover_speed: 20.0
# tuck pose (owner, hand-set demonstration 2026-09-27): shoulder far back (980), elbow NOT folded (48),
# wrist 133: upper arm stands with its motor resting on the joint-2 servo. Torque off. Joint 2 servo's
# stored limit is 960 -> drive to 960, then release to settle onto the rest.
arm_tuck_1: 500
arm_tuck_2: 980
arm_tuck_3: 48
arm_tuck_4: 133
arm_tuck_5: 499
arm_tuck_10: 629
# drive pose (owner name), set by hand with torque off and recorded 2026-09-27 16:48 (photo
# ~/model/pose_recorded_164827.jpg). Inside all mapped limits -> reachable by streamed trajectories.
# Unverified: whether it holds itself (torque off) while driving.
arm_drive_1: 500
arm_drive_2: 793
arm_drive_3: 51
arm_drive_4: 160 # owner 2026-09-29 "see more floor": camera -29 deg, floor 0.32-4.2 m ahead (was 179 = -24.5)
arm_drive_5: 499
arm_drive_10: 499
How the limits were found (2026-09-27). For each joint and direction the limit was opened to 0-1000 for one
sweep at 25 pulses/s, with the owner holding the gamepad and pressing a button if anything got close. The stop
position was read back and the limit set 20 pulses inside it (scripts/arm_sweep.py, raw data
docs/arm_sweeps.json). The limits are a union over the poses swept: joint 3 at 44 is fine with the arm folded and
was not with the arm straight up. A per-joint limit therefore does not guarantee a collision-free pose; the model
check in Step 3 covers combinations.
The named poses:
| Pose | 1 / 2 / 3 / 4 / 5 / claw | What it is for |
|---|---|---|
| rest | 500 / 776 / 211 / 161 / 500 / 499 | the default holding pose; ~/arm/recover goes here. Camera 0.38 m up, level +5 degrees |
| fold | 500 / 860 / 45 / 178 / 500 / 499 | the S-coil between rest and tuck, both ways; the path run daily since 2026-09-27 |
| tuck | 500 / 980 / 48 / 133 / 499 / 629 | torque off: the upper arm stands with its motor resting on the joint-2 servo. A reference, not a command target (joint 2 cannot pass 960) |
| drive | 500 / 793 / 51 / 160 / 499 / 499 | wheels only run in this pose. Camera -29 degrees, sees the floor 0.32-4.2 m ahead |
docs/motion_safety.md and the repo disagree in places; the yaml above is what runs (the robot's copy is identical to
the repo, survey_live_robot). The doc's "Default = 860/45/178" is today's fold pose; its drive table says wrist 179
(now 160); config/arm_poses.yaml "default" and the MoveIt SRDF state rest are also the fold pose.
Limits and arm_max_speed can be changed at run time with ros2 param set, but only while the arm is disabled; the
driver accepts 0 <= lo < hi <= 1000 and a speed of 1-300 pulses/s.
New idea: forward kinematics and collision checking
Forward kinematics computes where every link is from the joint angles: start at the base, apply each joint's
fixed origin and then its rotation, and multiply down the chain. With a box around each link, a collision
check asks whether any two boxes that are not neighbours overlap. Boxes are a coarse stand-in for the real
shapes; they are fast enough to check every 40 ms step of a move.
model/arm_model.py (110 lines, numpy only) does both from robot_model.json. The driver uses a copy,
ros2/rosorin_base/rosorin_base/arm_model.py, identical except one header line ("COPY of model/arm_model.py ... keep
in sync"). Its parts:
rot_rpy, rot_axis, T: 3x3 rotations and 4x4 transforms.ArmModel.q_from_pulses: pulses to radians with the factory zero (500) and flip.ArmModel.link_frames: chains base_joint, lidar_joint, joint1-joint5 and the fixed joints frombase_footprint and returns every link's 4x4 frame.ArmModel.box_corners: the 8 corners of each link's box in base_footprint.chassis_heights: the chassis surface height under any (x, y), from the 1 cm height map.obb_overlap: separating-axis test between two boxes (15 candidate axes, batched in one numpy array; 37 -> 8.7 mscollisions: the check itself.On the robot:
ARM_PARTS = ["link2", "link3", "link4", "link5", "gripper_link", "camera_link0", "camera_link"]
# pairs that are physically attached/adjacent (always in contact by construction) - not checked
ADJ = {frozenset(p) for p in [("link1", "link2"), ("link2", "link3"), ("link3", "link4"), ("link4", "link5"),
("link5", "gripper_link"), ("link4", "gripper_link"), ("link4", "camera_link0"),
("camera_link0", "camera_link"), ("link4", "camera_link")]}
def collisions(model, pulses, margin=0.005, samples=6):
q = model.q_from_pulses(pulses); F = model.link_frames(q); C = model.box_corners(F)
hits = []
parts = ["link1"] + ARM_PARTS
for i, a in enumerate(parts):
for b in parts[i + 1:] + ["lidar_frame"]:
if frozenset((a, b)) in ADJ or a == b:
continue
if a == "link1" and b == "lidar_frame":
continue # arm base sits on the lidar tower
if obb_overlap(C[a], C[b], margin):
hits.append((a, b))
g = np.linspace(0, 1, samples)
for a in ARM_PARTS: # arm vs chassis height map (vectorised 2026-09-29:
P = C[a]; o = P[0]; e1, e2, e3 = P[4] - P[0], P[2] - P[0], P[1] - P[0] # same result, ~10x faster)
pts = (o + g[:, None, None, None] * e1 + g[None, :, None, None] * e2 + g[None, None, :, None] * e3).reshape(-1, 3)
if (pts[:, 2] <= model.chassis_heights(pts[:, 0], pts[:, 1]) + margin).any():
hits.append((a, "chassis"))
return hits
Every pair of arm parts is tested against each other and against the LiDAR housing, with a 5 mm margin; then a 6 x
6 x 6 grid of points inside each arm box is compared with the chassis height under it. The result is a list of
touching pairs; empty means clear.
Where the driver uses it: every waypoint and segment midpoint of a MoveIt trajectory, every new MoveIt Servo target,
and every JointJog step that changes the commanded pose. Each time it drops one pair:
{link5, camera_link0}. Any roll of joint 5 flags that pair, and it is a known false positive of the boxes.
How far to trust it (docs/motion_safety.md, validated 2026-09-27):
| Check | Reality | Model |
|---|---|---|
| joints 2/3/4 at 500/500/500 | straight up (camera + read-back) | straight up, tip 0.522 m |
| joint 2 back from rest | free to 961 | contact at 896: too conservative |
| joint 2 forward from rest | owner stop at 562 | contact at 429 (gripper vs chassis front) |
The model does not include the claw's fingers opening and closing: gripper_link is one fixed box. On 2026-10-05 the
owner found that the claw physically hits the joint-2 servo in the S-coil, and the driver had stopped 178 jog steps
that day for (link1|link2, gripper_link). That is why the rest pose moved out of the S-coil.
Compute a pose yourself on the robot (no motion, no ROS):
On the robot:
cd ~/ros2_ws/src/rosorin_base
python3 -c "
from rosorin_base import arm_model
M = arm_model.ArmModel('config/robot_model.json')
for name, p in (('rest', {1: 500, 2: 776, 3: 211, 4: 161, 5: 500, 10: 499}), ('drive', {1: 500, 2: 793, 3: 51, 4: 160, 5: 499, 10: 499})):
F = M.link_frames(M.q_from_pulses(p))
print(name, F['camera_link'][:3, 3].round(3), arm_model.collisions(M, p))
"
Check
Computed with the repo code for this guide (2026-10-07), not taken from the robot's log:rest [0.038 0. 0.383] [] drive [0.099 0.001 0.316] [('link5', 'camera_link0')]The camera is 0.383 m above the floor plane in the rest pose (the yaml comment says 0.38 m). The drive pose
shows the known false positive, which the driver ignores. For the tucked pose read on 2026-09-28 (500/981/45/129/
499) the model putscamera_linkat (0.023, 0.001, 0.314): 0.289 m abovebase_link, the same value
tf2_echoprinted in chapter 12.
ros2/rosorin_base/rosorin_base/board_driver.py (1138 lines) owns the board, wheels and arm alike. It is too big to
type in pieces; this step explains the arm parts and shows the key functions. The full file is in the repo.
From docs/motion_safety.md (approved 2026-09-27):
board_driver.arm/goal latches the e-stop, like a second one on cmd_vel.On the robot:
ARM_IDS = (1, 2, 3, 4, 5, 10)
# The claw (10) is not a joint of the arm: nothing the head, the drive pose or the wheels need depends on where it is.
# 2026-10-05: it stopped answering and every pose move (wake-up, drive pose -> wheel enable) refused for hours.
# A silent optional servo is carried at its last known pulse, reported in state (servo_silent) and logged.
ARM_OPTIONAL = (10,)
FOLLOW_TIMEOUT = 0.25 # s without a Servo command -> stop following (hold)
JOG_TIMEOUT = 0.2 # s without a JointJog -> decelerate to a stop
JOG_ACCEL = 6.0 # rad/s^2: glide up/down, no steps (owner 2026-10-01: 3.0 -> 6.0, head too slow)
TUCK_TOL = 25 # pulses: joints 2-4 read-back vs owner's tuck (servo 2 capped at 960 vs 980)
SERVO2_STORED_MAX = 960 # servo 2 EEPROM angle limit (read 0x32: 80..960)
ARM_SETTLE_TOL = 3 # pulses; tolerance on move targets in slow single moves
PLUG_STEP_MV = 190 # mV one-step jump with wheels off = charger plugged/unplugged (log 2026-10-01: +249 / -226)
ARM_PICKUP_TOL = 10 # pulses; tolerance on the CURRENT pose at arm enable only (owner 2026-10-01; gravity sag)
# read-only servo queries: cmd -> (reply struct, name)
SERVO_READS = {0x05: ('<BBbh', 'pos'), 0x07: ('<BBbH', 'vin_mv'), 0x09: ('<BBbB', 'temp_c'),
0x0D: ('<BBbb', 'torque')}
docs/motion_safety.md still says the JointJog ramp is 3 rad/s^2; the code runs 6.0 since 2026-10-01.
The serial thread puts every function-5 reply on servo_q; servo_query sends one read and waits up to 0.3 s for
the matching reply:
On the robot:
def servo_query(self, sid, cmd, timeout=0.3):
"""Read-only query of one bus servo value; None on timeout/failure."""
fmt, _ = SERVO_READS[cmd]
while not self.servo_q.empty():
self.servo_q.get_nowait()
self.write(encode_frame(FUNC_BUS_SERVO, bytes([cmd, sid])))
end = time.monotonic() + timeout
while time.monotonic() < end:
try:
p = self.servo_q.get(timeout=max(0.0, end - time.monotonic()))
except queue.Empty:
break
if len(p) == struct.calcsize(fmt) and p[0] == sid and p[1] == cmd:
rid, rcmd, ok, val = struct.unpack(fmt, p)
if ok == 0 and cmd == 0x05:
self.joint_pulses[sid] = val
return val if ok == 0 else None
return None
read_pose reads all six and applies fill_optional with ARM_OPTIONAL; note_silent logs a change in the set of
silent servos once (arm servos not answering: [10] (optional: arm and wheels carry on)) and publishes it as
servo_silent in ~/state.
On the robot:
def arm_hold(self, why):
# always on e-stop: single-command moves (recover/tuck/drive) run while arm_enabled is False
if self.arm_traj is not None or self.arm_enabled or why == 'e-stop':
self.write(arm_stop_frame(ARM_IDS))
self.get_logger().info(f'arm hold ({why})')
self.arm_traj = None
self.arm_enabled = False
self.follow = None
self.jog = None
if self.fjt:
self.fjt_finish('canceled' if why == 'trajectory canceled' else f'stopped: {why}')
Every e-stop (topic, any gamepad button except MODE, a second publisher) calls arm_hold('e-stop'), which always
sends the hold frame. Before that rule, the single-command moves below could not be stopped by the gamepad because
they run with arm_enabled False (docs/motion_safety.md, tuck/drive modes, 2026-09-27).
arm/goal takes a sensor_msgs/JointState whose name fields are servo IDs as strings and whose position fields
are pulses. The callback checks, refuses or starts a trajectory:
On the robot:
def on_arm_goal(self, m):
if not self.arm_enabled or self.gate.estop is not None:
self.get_logger().warn('arm goal ignored: arm not enabled or e-stop latched')
return
try:
goal = {int(n): int(round(p)) for n, p in zip(m.name, m.position)}
except ValueError:
self.get_logger().warn(f'arm goal ignored: names must be servo ids, got {list(m.name)}')
return
why = check_goal(goal, self.arm_limits)
if why:
self.get_logger().warn(f'arm goal refused: {why}')
return
start = {sid: self.arm_pose[sid] for sid in goal}
if self.fjt:
self.fjt_finish('preempted by arm/goal')
self.arm_traj = Trajectory(start, goal, self.arm_speed, time.monotonic())
self.arm_activity(holding=True)
self.get_logger().info(f'arm move {start} -> {goal} in {self.arm_traj.T:.2f} s')
and the 50 Hz control loop sends a sample on every second tick (25 Hz):
On the robot:
if self.arm_traj is not None and self.arm_tick % 2 == 0: # 25 Hz streaming
targets = self.arm_traj.at(now)
self.write(arm_move_frame(targets, 40))
self.arm_active_t = now
self.arm_pose.update(targets)
self.joint_pulses.update(targets)
if self.arm_traj.done(now):
if self.fjt and self.fjt[2] is self.arm_traj:
self.fjt_finish('ok')
self.arm_traj = None
Streaming needs a known start inside the limits. After a power cut the arm sags and can read outside 0-1000 (servo 3
read -35 on 2026-09-27), and the tucked pose is outside the limits on purpose. For those cases the driver sends one
move command and lets the servo interpolate from wherever it actually is:
~/arm/recover: to the rest pose. From the known tuck it goes by way of the fold pose at arm_untuck_speedarm_recover_speed 20 pulses/s,~/arm/drive: to the drive pose. Base joint 1 stays where it is unless the parameter drive_pan is set~/arm/tuck: two moves and a release, then a self-check from servo read-back, no camera:On the robot:
def on_arm_tuck(self, req, res):
"""TUCK (owner-demonstrated 2026-09-27): slow move to DEFAULT, then slow move to the tuck approach
(owner's tuck with joint 2 capped at the servo's stored max 960, claw closed), then torque off 1-5.
The pose rests on itself (upper arm motor on the joint-2 servo): releasing changes nothing.
Self-check from servo read-back vs the owner's tuck (no camera)."""
if not self._named_ok(res):
return res
start = self.read_pose()
default = dict(self.arm_fold) # fold (S-coil) first, then back onto the rest
if start.get(1) is not None:
default[1] = start[1] # base does not need to move (owner)
default[10] = self.arm_limits[10][1] # claw closed
approach = dict(self.arm_tuck)
approach[2] = min(approach[2], SERVO2_STORED_MAX) # servo 2 refuses to drive past its stored limit
approach[10] = self.arm_limits[10][1]
approach[1] = default[1]
for target, why in ((default, 'tuck 1/2: to FOLD + claw closed'), (approach, 'tuck 2/2: to TUCK approach')):
tol = {sid: (lo - ARM_SETTLE_TOL, hi + ARM_SETTLE_TOL) for sid, (lo, hi) in self.arm_limits.items()}
tol[2] = (tol[2][0], SERVO2_STORED_MAX)
bad = check_goal(target, tol)
if bad:
res.success, res.message = False, f'refused: {why}: {bad}'
return res
pose = self.read_pose()
dist = max(abs(target[k] - pose[k]) for k in ARM_IDS if pose[k] is not None)
# normal start (every joint read and inside limits +/- tol): faster tuck (owner 2026-09-29); else slow
normal = None not in pose.values() and check_goal(pose, tol) is None
dur = max(1.0, dist / self.arm_tuck_speed) if normal else max(3.0, dist / self.arm_recover_speed)
self.get_logger().info(f'arm {why}: {pose} -> {target} in {dur:.1f} s')
self.busy_pub.publish(Bool(data=True))
self.write(arm_move_frame(target, int(dur * 1000)))
time.sleep(dur + 0.5)
self.busy_pub.publish(Bool(data=False))
relax = self.on_arm_relax(req, Trigger.Response())
time.sleep(1.0)
settled = self.read_pose()
torque = {sid: self.servo_query(sid, 0x0D) for sid in ARM_IDS}
tucked = all(settled.get(j) is not None and abs(settled[j] - self.arm_tuck[j]) <= TUCK_TOL for j in (2, 3, 4))
res.success = tucked and all(torque[j] == 0 for j in (1, 2, 3, 4, 5))
res.message = json.dumps({'tucked': tucked, 'settled': settled, 'tuck_ref': self.arm_tuck,
'torque': torque, 'start': start})
self.get_logger().info(f'arm TUCK result: {res.message}')
self.arm_activity(holding=not res.success) # failed tuck: still holding, retried after auto_tuck_s
return res
These moves block the driver's executor while they run (time.sleep), so no joint states are published and the
camera transform stands still while the camera moves. The driver says so on ~/arm_busy (a latched Bool) before
and after; the depth gate of chapter 18 drops depth frames while it is true (docs/lessons.md 2026-10-02).
On the robot:
def auto_tuck_check(self):
"""1 Hz: tuck an arm left holding with no activity for auto_tuck_s (wheels disabled, no e-stop, not moving)."""
if self.gate.enabled:
self.arm_active_t = time.monotonic() # driving (arm in drive pose) counts as activity
if self.auto_tuck_s <= 0 or not self.arm_holding or self.arm_traj is not None:
return
if self.gate.enabled or self.gate.estop is not None:
return
if time.monotonic() - self.arm_active_t < self.auto_tuck_s:
return
self.get_logger().info(f'AUTO-TUCK: arm idle {self.auto_tuck_s:.0f} s while holding')
if self.arm_enabled:
self.arm_hold('auto-tuck') # tuck requires the arm disabled (holding)
res = self.on_arm_tuck(Trigger.Request(), Trigger.Response())
if not res.success:
self.get_logger().warn(f'AUTO-TUCK failed, retry after {self.auto_tuck_s:.0f} s: {res.message[:200]}')
"Holding" means torque on in a pose; recover, drive, stiffen, enable and every accepted goal set it and reset the
clock; tuck and relax clear it. After a driver restart the state is unknown and counts as not holding until the next
arm activity. The owner measured that tuck roughly doubles battery life; the servos also stop heating.
New idea: velocity commands for a joint
control_msgs/JointJogcarries joint names and velocities (rad/s) instead of positions. A tracker that wants
to follow a face sends "pan left at 0.3 rad/s" many times a second; when the messages stop, the joint should glide
to a stop rather than freeze. The robot's mind (chapter 21) moves the head only this way: the owner ruled out
position-jump arm moves ("way too jerky",docs/decisions.md).
The driver integrates JointJog on arm_controller/joint_jog at 25 Hz: velocity ramps at JOG_ACCEL 6 rad/s^2 toward
the command, capped at arm_jog_max_speed 300 pulses/s (1.26 rad/s, 72 degrees/s); no message for 0.2 s
(JOG_TIMEOUT) means glide to zero; a joint that reaches its limit stops there. Each step that changes the
commanded pose is checked against the model first:
On the robot:
if out != {k: self.arm_pose[k] for k in out}:
full = dict(self.arm_pose); full.update(out)
hits = [h for h in arm_model.collisions(self.fjt_model, full) if set(h) != {'link5', 'camera_link0'}]
if hits: # would collide: stop here, keep the last safe pose
self.get_logger().warn(f'jog stopped by arm model veto: {hits}', throttle_duration_sec=2.0)
j['pos'] = {k: float(self.arm_pose[k]) for k in j['pos']}
j['vel'] = {k: 0.0 for k in j['vel']}
return
self.write(arm_move_frame(out, 40))
self.arm_pose.update(out)
The check runs only when the pose changes: the mind streams zero-velocity jogs while it holds still, and checking an
unchanged pose 25 times a second had the driver's main thread at 60 % CPU (owner, 2026-10-01).
Services (all under /rosorin_board_driver/):
| Service | Type | What it does | Refused when |
|---|---|---|---|
arm/read_state |
Trigger | reads position, voltage, temperature, torque of all six; JSON in message |
wheels enabled |
arm/recover |
Trigger | one move to rest (via fold from tuck); does not enable | arm or wheels enabled, e-stop, a joint servo silent |
arm/enable |
SetBool | true: read the pose (must be within limits +-10 pulses, ARM_PICKUP_TOL), take the arm for streaming; false: hold |
e-stop, wheels enabled, pose read failed, pose outside |
arm/drive |
Trigger | one move to the drive pose | arm or wheels enabled, e-stop |
arm/tuck |
Trigger | fold + claw closed, tuck approach, torque off, self-check | arm or wheels enabled, e-stop |
arm/relax |
Trigger | torque off on all six (0x0C), verified with 0x0D, up to 3 attempts | arm or wheels enabled |
arm/pretuck |
Trigger | move to the tuck clamped into the limits, servos stay on | arm or wheels enabled, e-stop |
arm/stiffen |
Trigger | torque on in place (commands the current pose for 1 s) | arm or wheels enabled, e-stop, pose outside 0-1000 |
Topics and actions: arm/goal (JointState, pulses), arm_controller/joint_jog (JointJog, rad/s),
arm_controller/follow_joint_trajectory (FollowJointTrajectory action, for MoveIt), arm_controller/joint_trajectory
(MoveIt Servo, parked), ~/arm_busy (latched Bool), /joint_states (chapter 12) and ~/state (JSON with
arm_enabled, arm_moving, arm_pose, servo_silent, arm_holding, auto_tuck_in_s).
Wheel enable (~/enable, chapter 11) takes the arm, moves it to the drive pose and refuses to enable the wheels if
the arm is not within 30 pulses of it afterwards (2026-10-05 fix, below).
These services and topics were exercised on this robot with the owner watching, between 2026-09-27 and 2026-10-05.
Where a check quotes a value computed from the code instead of a recorded output, it says so.
Safety
- Watch the arm for every command. Keep your hands out of its reach. Have the gamepad switched on and in your
hand: any button except MODE latches the e-stop, which holds the servos where they are (hold MODE 2 s to
clear; the wheels stay disabled).- Never call
arm/relaxunless the arm rests on itself. Torque off drops every joint that carries weight; tuck is
the only pose that is designed for torque off.- Before you cut the power, tuck the arm or support it. With power gone the servos go limp and the arm sags.
- The claw (10) refuses torque off: it keeps reporting torque on after 3 attempts with 0x0C. Unresolved.
- If
rosorin-mindis installed (chapter 21) it holds the arm. Stop it for these tests and start it afterwards:
sudo systemctl stop rosorin-mind, latersudo systemctl start rosorin-mind.
In every robot shell:
On the robot:
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
On the robot:
timeout 15 ros2 service call /rosorin_board_driver/arm/read_state std_srvs/srv/Trigger | grep -o "message=.*" | cut -c1-600
Check
Recorded on 2026-10-01 (arm holding the fold pose, on battery):message='{"1": {"pos": 501, "vin_mv": 11100, "temp_c": 32, "torque": 1}, "2": {"pos": 862, "vin_mv": 11100, "temp_c": 30, "torque": 1}, "3": {"pos": 36, "vin_mv": 11000, "temp_c": 33, "torque": 1}, "4": {"pos": 177, "vin_mv": 11000, "temp_c": 34, "torque": 1}, "5": {"pos": 500, "vin_mv": 11000, "temp_c": 28, "torque": 1}, "10": {"pos": 499, "vin_mv": 11001, "temp_c": 30, "torque": 1}}')All six answer. Servo 3 reads 36 where 45 was commanded: the elbow carries the load and sags about 9 pulses.
If it fails
refused: wheel motion enabled (reads would stall the deadman): disable the wheels first
(ros2 service call /rosorin_board_driver/enable std_srvs/srv/SetBool "{data: false}").- A servo shows
{"pos": null, "vin_mv": null, "temp_c": null, "torque": null}: it is not answering. On
2026-10-05 the claw did this from 19:44 UTC until a power cycle. If it is servo 10, the arm carries on; if it is
a joint servo, every pose move refuses until it answers again. Check the cable before anything else.
On the robot:
timeout 40 ros2 service call /rosorin_board_driver/arm/recover std_srvs/srv/Trigger
journalctl -u rosorin-base --since "-1 min" --no-pager -o cat | grep "arm recover"
Check
success=True. From the tuck the journal shows the two steps (2026-10-05):arm recover {1: 499, 2: 963, 3: 45, 4: 131, 5: 498, 10: 629} -> {1: 500, 2: 860, 3: 45, 4: 178, 5: 500, 10: 499} in 1.1 s (single command, untuck from tuck) arm recover {1: 500, 2: 860, 3: 45, 4: 178, 5: 500, 10: 499} -> {1: 500, 2: 776, 3: 211, 4: 161, 5: 500, 10: 499} in 1.4 s (single command, untuck from tuck)followed by
arm recover result:withbefore,after,duration_sand"inside_limits": true. After a power
cut (sagged arm) it takes one slow move of at least 3 s instead: on 2026-09-27 it brought servo 3 from -21 to 40
in 3.7 s.
On the robot:
timeout 10 ros2 service call /rosorin_board_driver/arm/enable std_srvs/srv/SetBool "{data: true}"
timeout 10 ros2 topic pub --once -w 0 /arm/goal sensor_msgs/msg/JointState "{name: ['4'], position: [220.0]}"
sleep 3
timeout 10 ros2 topic pub --once -w 0 /arm/goal sensor_msgs/msg/JointState "{name: ['4'], position: [100.0]}"
sleep 1
timeout 10 ros2 service call /rosorin_board_driver/arm/enable std_srvs/srv/SetBool "{data: false}"
journalctl -u rosorin-base --since "-1 min" --no-pager -o cat | grep -E "arm (enabled|move|goal|hold)"
The first goal tilts the wrist to 220 (the command was used on 2026-09-29 to take camera frames at wrist 220 and
260). The second is below the wrist's limit of 121.
Check
The enable answersarm enabled at {1: 500, 2: 776, ...}with the pose it read. In the journal:
arm move {4: 161} -> {4: 220} in 0.44 s(format from the code; the time is 1.875 x 59 / 250 for a start at
the rest pose's 161), thenarm goal refused: servo 4 goal 100 outside limits [121, 935], then
arm hold (disable). The first such test on 2026-09-27 (with the early narrow limits) logged, as recorded:arm enabled at {1: 500, 2: 724, 3: 46, 4: 149, 5: 501, 10: 499} arm goal refused: servo 1 goal 700 outside limits [450, 550] arm hold (disable)Nothing moved on the refused goal, and there was no twitch on hold.
If it fails
refused: current pose outside limits (+/-10): the arm sagged past a limit while holding. On 2026-10-01 servo
3 sat at 36 against its limit of 40 and the mind retried enable every second all day. Runarm/recoverfirst;
the mind now does the same, with a 30 s back-off.arm goal ignored: names must be servo ids:namemust be the servo ID as a string ('4'), notjoint4.- The driver latches an e-stop
multiple arm/goal publishers: another node publishes onarm/goal. Only one may.
On the robot:
timeout 90 ros2 service call /rosorin_board_driver/arm/tuck std_srvs/srv/Trigger | grep -o "success=[A-Za-z]*\|\"tucked\": [a-z]*\|\"settled\": {[^}]*}"
Check
Recorded on 2026-10-05:success=True "tucked": true "settled": {"1": 499, "2": 963, "3": 45, "4": 131, "5": 498, "10": 629}Joints 2-4 within 25 pulses of the tuck reference (980/48/133), torque 0 on servos 1-5.
Now recover again and leave the arm alone. Watch the countdown in the driver state:
On the robot:
timeout 40 ros2 service call /rosorin_board_driver/arm/recover std_srvs/srv/Trigger >/dev/null
for i in 1 2 3; do timeout 6 ros2 topic echo --once --field data /rosorin_board_driver/state | grep -o '"auto_tuck_in_s": [0-9.]*'; sleep 30; done
Check
The countdown falls from about 120 s. On the stand on 2026-09-29: "recover -> countdown 118 s ... -> AUTO-TUCK at
120 s -> tucked=true (settled 501/960/46/134)". The journal showsAUTO-TUCK: arm idle 120 s while holding.
The state topic is silent for the length of the tuck, because the tuck runs inside the driver's executor.
If it fails
tucked: falsewith the arm standing upright: the tuck did not settle onto the joint-2 servo. Twice in the
record, tuck was reported done from the wrong criteria while the arm stood upright; that is why the success
test is the servos' own read-back now. Three guessed tuck poses failed before the owner set it by hand. If the
tuck fails, the driver keeps "holding" and tries again after another 120 s.- No countdown (
auto_tuck_in_snull): the arm is not "holding" (for example right after a driver restart),
orauto_tuck_sis 0.
On the robot:
timeout 40 ros2 service call /rosorin_board_driver/arm/drive std_srvs/srv/Trigger
Check
success=Trueand a JSON message withbefore,afterandduration_s;afterreads about 793/51/160 on
joints 2-4. On 2026-10-05 the wheel enable path did this from an arm held by the mind: "taken, moved in 1.0 s to
pan 494 / wrist 160, wheels enabled". The elbow sags in this pose (51 commanded, 41-42 read on 2026-09-27).
scripts/arm_jog_test.py recovers if needed, enables the arm, sends JointJog at 50 Hz (pan +0.3 rad/s for 2 s,
-0.3 for 4 s, +0.3 for 2 s, then wrist -0.2 and +0.2), disables, and prints what /joint_states saw. Copy it to the
robot and run it with the arm free to pan:
On your laptop:
scp ~/CCode/rosorin-pro/scripts/arm_jog_test.py rosorin:setup/
On the robot:
python3 ~/setup/arm_jog_test.py
Check
docs/motion_safety.md(stand, 2026-09-29): "pan +-0.3 rad/s sweeps -34..+37 deg (expected +-34)", and the veto
"stopped a wrist tilt from the rest pose that would have put the gripper into links 1/2". The first run, before
the veto was made faster, loggedjog stopped by arm model veto: [('link1', 'gripper_link'), ('link2', 'gripper_link')]andjoint_states 301 msgs in 12.0 s (25 Hz).
The gamepad can jog too (docs/motion_safety.md, gamepad modes): MODE cycles locked -> drive -> arm -> locked; in
arm mode the D-pad left/right selects joint 1-5, the right stick jogs it (at most 80 pulses/s, clamped to the limits),
the left stick moves the claw. Entering arm mode from the tuck untucks first.
The claw went silent and stopped arm and wheels (2026-10-05)
The claw servo (10) stopped answering at 19:44 UTC. The mind kept tracking with the arm it already held. At 20:03
the mind was stopped for a test; restarted, it could not take the arm again, because every move to a named pose
read all six servos and refused if one was silent. The same refusal blocks wheel enable (drive pose first).
Nobody noticed for hours. The driver log then showed the claw as all-null:arm read: {"1": {"pos": 54, ...}, ..., "10": {"pos": null, "vin_mv": null, "temp_c": null, "torque": null}}Fix the same day:
ARM_OPTIONAL = (10,)andfill_optional- a silent claw is carried at its last known pulse,
logged and reported inservo_silent; a silent joint servo still refuses. Four tests intest_arm.pycover it.
Likely cause, not proven: in the S-coil the claw pressed against the joint-2 servo (178 veto stops that day)
until the claw servo shut off; it came back only after a power cycle. Rule taken from it: after stopping or
restarting any robot service for a test, read back that the robot is in the state it was in before.
An exploration run drove with the arm stretched sideways (2026-10-05)
Wheel enable only moved the arm to the drive pose when the arm was not held. The mind was tracking the owner when
the wheels were enabled, so the step was skipped without a log line, and the robot drove 4 m with pan 36 / wrist
356 instead of 500 / 160. Fix in the driver: wheel enable always takes the arm, moves it, checks it, and refuses
if it is not in the drive pose.
The arm sags after a power cut
Servos lose torque with main power. On 2026-09-27 after power-on servo 3 read -35, outside its own 0-1000 command
range, and kept sagging (2=658, 3=-21, 4=223). Enable correctly refused. A stream cannot start from there
(negative pulses do not fit the u16 command; clamping would jump), hence the single slow move ofarm/recover.
Private limits in another program (2026-10-01)
The mind kept its own head-tilt range 150-280 on top of the owner's mapped 121-935, and the owner had to point it
out. Limits come fromarm_limits.yamlonly.
The factory stack's arm incident
/servo_controlleron the factory image had 7+ publishers and no arbitration, which caused a real arm incident
(docs/lessons.md, factory section). Hence one owner and onearm/goalpublisher.
MoveIt plans arm paths around obstacles. It was installed, configured and tested on the stand on 2026-09-29, then
parked on 2026-10-01 (docs/decisions.md, simplified plan). On 2026-10-02 the owner ruled out scripted arm scans and
position-jump moves: all head motion is the mind's JointJog path. MoveIt Servo (the streaming part) ran at about 1/6
of the commanded speed for a reason never found. Nothing starts MoveIt at boot.
scripts/install_moveit.sh installs from the ROS apt repo and records exactly what it added, so
scripts/rollback_moveit.sh can remove only that:
On the robot:
#!/bin/bash
# MoveIt 2 + Servo from the official ROS apt repo (2026-09-29): arm planning/named poses/smooth servoing.
# Records the exact package set added so rollback_moveit.sh removes only what this added.
set -e
before=$(mktemp); dpkg-query -W -f='${Package}\n' | sort > "$before"
sudo apt-get install -y ros-humble-moveit ros-humble-moveit-servo ros-humble-moveit-ros-perception
dpkg-query -W -f='${Package}\n' | sort | comm -13 "$before" - > ~/moveit_added_packages.txt
echo "added $(wc -l < ~/moveit_added_packages.txt) packages (list: ~/moveit_added_packages.txt)"
(On the robot it was run in two parts on 2026-09-29: moveit + moveit-servo first, moveit-ros-perception at
23:43, 92 packages tracked in the list.)
The configuration is the data-only package ros2/rosorin_moveit:
config/rosorin.srdf: planning group arm = chain base_link to link5; named states rest, drive,look_down_a, look_down_b, touch_your_toes; adjacent pairs and link5/camera_link0 excluded fromrest state is the fold pose (joint2 -1.50796 rad = pulse 860), not today's rest.config/joint_limits.yaml: the mapped pulse limits converted to radians, 1.047 rad/s, default_robot_paddingconfig/kinematics.yaml: KDL with position_only_ik: true (a 5-joint arm cannot reach arbitrary 6-D poses).config/moveit_controllers.yaml: one controller, arm_controller, type FollowJointTrajectory: MoveIt executesconfig/sensors_3d.yaml: an octomap from the depth camera's /aurora/points2.launch/move_group.launch.py: loads rosorin_base's generated URDF and starts move_group.Build and start it by hand:
On the robot:
cd ~/ros2_ws && source /opt/ros/humble/setup.bash && colcon build --packages-select rosorin_moveit
source ~/ros2_ws/install/setup.bash
nohup ros2 launch rosorin_moveit move_group.launch.py > /tmp/move_group.log 2>&1 &
sleep 15; grep -i "error\|fatal\|ready\|loaded\|warn" /tmp/move_group.log | head -20
Then scripts/moveit_named_move.py recovers and enables the arm and asks move_group for drive and then rest.
Check
Recorded 2026-09-29:move_grouploggedLoaded robot model in 0.0147391 seconds. The same log had two
Cannot infer URDF/SRDF from ...warnings from the config builder (the launch file names both files
explicitly) andResolution not specified for Octomap(that run was beforeoctomap_resolution0.02 was added
to the launch file). The named-move run:enable: True arm enabled at {1: 501, 2: 862, 3: 44, 4: 177, 5: 499, 10: 499} drive: MoveIt error_code 1 (SUCCESS) in 2.7 s, pose {'1': 498, '2': 791, '3': 52, '4': 158, '5': 502, '10': 499} rest: MoveIt error_code 1 (SUCCESS) in 2.7 s, pose {'1': 501, '2': 859, '3': 46, '4': 178, '5': 502, '10': 499} disable: arm disabled (holding)11 trajectory points each, both poses within about 2 pulses. A cross-check of MoveIt's collision model against
the arm model agreed on 99.6 % of 1500 random poses (scripts/moveit_collision_xcheck.py).
Not test-built
Whethermove_groupstill starts cleanly on the current robot is not recorded after 2026-09-29; there is no
service unit for it. MoveIt Servo is parked at about 1/6 speed, cause unknown. Stopmove_groupwhen you are done
(pkill -f "[l]ib/moveit_ros_move_group/move_group"); it is not part of the running system.
python3 -m pytest -q test/test_arm.py ends with 15 passed.arm/read_state lists all six servos with a position, a voltage near the battery's, and torque state.arm/recover reaches the rest pose with "inside_limits": true.arm/tuck returns "tucked": true, and an untouched arm tucks itself 120 s after a recover.Where this comes from
Repo (branchrebuild,bfb61d8):ros2/rosorin_base/rosorin_base/arm.py,arm_model.py
(=model/arm_model.py),board_driver.py(constants lines 53-68;servo_query,read_pose,on_arm_read,
arm_hold,on_arm_enable,on_arm_recover,on_arm_relax,on_arm_drive,on_arm_tuck,on_arm_goal,
jog_step,auto_tuck_check,publish_state),config/arm_limits.yaml,config/arm_poses.yaml,
test/test_arm.py,ros2/rosorin_moveit/(SRDF, joint_limits, kinematics, controllers, sensors_3d,
move_group.launch.py),scripts/install_moveit.sh,scripts/rollback_moveit.sh,scripts/arm_jog_test.py,
scripts/moveit_named_move.py,scripts/arm_sweep.py. Docs:docs/motion_safety.md(arm design 2026-09-27,
stand tests, limit mapping, power-off, collision model, tuck final, auto-tuck, drive pose and gamepad modes,
MoveIt execution path),docs/hardware.md(arm, claw collision 2026-10-05),docs/lessons.md(factory
/servo_controller; 2026-09-27 servo lessons; 2026-10-01 private limits and sag; 2026-10-02 camera TF; 2026-10-05
arm sideways, claw silent),docs/decisions.md2026-10-01 (MoveIt parked) and 2026-10-02 morning (position-jump moves out),
docs/status.md(Servo parked; unit tests not registered). Command log: arm test 1 2026-09-27 08:17, read_state
2026-09-27 08:14 and 2026-10-01 20:00, recover + wrist 220/260 2026-09-29 06:38, tuck 2026-09-29 20:43 and
2026-10-05 23:14, recover lines 2026-10-05, claw null reads 2026-10-05 20:59, move_group launch 2026-09-29 23:39,
named moves 2026-09-29, JointJog first run 2026-09-30 00:03, MoveIt apt installs 2026-09-29 23:26/23:43. Frame
bytes and the rest/drive camera positions were computed for this guide on 2026-10-07 with the repo code.