"""Diagnose post-motion wobble on Feetech servos. Moves each motor to a goal, waits for ReadMoving() to clear, then samples (position, speed, load, current, voltage, moving) at --rate Hz for --hold-time seconds. Prints a time-series tail plus a per-motor summary so you can see which joint is actually oscillating, whether the PID is hunting (non-zero load at rest) or the mechanics are (zero load, mass swinging), and whether the bus voltage is drooping. Usage: python diag_motor_wobble.py [motor_id ...] [--goals r1,r2,...] [--speed N] [--accel N] [--hold-time S] [--rate HZ] Default goal is DEFAULT_GOALS_RAD from write_motors_from_goals.py style. """ import argparse import math import sys import time from scservo_sdk import PortHandler import scservo_patch from scservo_sdk.sms_sts import sms_sts USB_PORT = "/dev/tty.usbserial-0001" BAUD_RATE = 115200 TICKS_PER_REV = 4096 CENTER_TICK = 2048 TICKS_TO_RAD = (2.0 * math.pi) / TICKS_PER_REV RAD_TO_TICKS = TICKS_PER_REV / (2.0 * math.pi) SAFE_TICK_LO = CENTER_TICK - 1251 SAFE_TICK_HI = CENTER_TICK + 1251 DEFAULT_SPEED = 360 DEFAULT_ACCEL = 120 ADDR_TORQUE_ENABLE = 40 ADDR_LOAD = 60 ADDR_VOLTAGE = 62 ADDR_TEMP = 63 ADDR_CURRENT = 69 DEFAULT_GOALS_RAD = [-0.11, -0.38, 0.03, -0.12, 0.09, 0.0, 0.0] def rad_to_ticks(rad, sign): return int(round(CENTER_TICK + sign * rad * RAD_TO_TICKS)) def parse_args(): p = argparse.ArgumentParser() p.add_argument("motor_ids", nargs="+", type=int) p.add_argument("--goals", default=None) p.add_argument("--signs", default=None) p.add_argument("--speed", type=int, default=DEFAULT_SPEED) p.add_argument("--accel", type=int, default=DEFAULT_ACCEL) p.add_argument("--hold-time", type=float, default=3.0, help="Seconds to sample after motors report stopped.") p.add_argument("--rate", type=float, default=50.0) p.add_argument("--settle-timeout", type=float, default=10.0) p.add_argument("--fast", action="store_true", help="Read only pos+moving per sample for higher effective rate.") return p.parse_args() def emergency_stop(sts, motor_ids): for mid in motor_ids: try: sts.writeTxRx(mid, ADDR_TORQUE_ENABLE, 1, [0]) except Exception: pass def read_signed_load(sts, mid): # 10-bit signed (bit 10 = sign, bits 0..9 = magnitude). raw, comm, _ = sts.read2ByteTxRx(mid, ADDR_LOAD) if comm != 0: return None return -(raw & 0x3FF) if raw & 0x400 else (raw & 0x3FF) def read_voltage(sts, mid): raw, comm, _ = sts.read1ByteTxRx(mid, ADDR_VOLTAGE) return None if comm != 0 else raw * 0.1 def read_current_ma(sts, mid): raw, comm, _ = sts.read2ByteTxRx(mid, ADDR_CURRENT) return None if comm != 0 else raw * 6.5 def read_temp(sts, mid): raw, comm, _ = sts.read1ByteTxRx(mid, ADDR_TEMP) return None if comm != 0 else raw def read_speed(sts, mid): raw, comm, _ = sts.ReadSpeed(mid) if comm != 0: return None # SDK may not sign-extend on all firmwares; be explicit. if raw > 0x7FFF: return -(raw & 0x7FFF) return raw def main(): args = parse_args() n = len(args.motor_ids) if args.goals: goals_rad = [float(x) for x in args.goals.split(",")] else: if n > len(DEFAULT_GOALS_RAD): sys.exit(f"DEFAULT_GOALS_RAD only has {len(DEFAULT_GOALS_RAD)} values") goals_rad = DEFAULT_GOALS_RAD[:n] if len(goals_rad) != n: sys.exit(f"{len(goals_rad)} goal values but {n} motor IDs") signs = [1] * n if args.signs: signs = [int(s) for s in args.signs.split(",")] if len(signs) != n: sys.exit("--signs length must match number of motor IDs") goals_ticks = [] for mid, rad, sgn in zip(args.motor_ids, goals_rad, signs): if not math.isfinite(rad): sys.exit(f"Goal for motor {mid} is not finite: {rad!r}") ticks = rad_to_ticks(rad, sgn) if ticks < SAFE_TICK_LO or ticks > SAFE_TICK_HI: sys.exit( f"Goal for motor {mid} = {ticks} ticks is outside safe range " f"[{SAFE_TICK_LO}, {SAFE_TICK_HI}]" ) goals_ticks.append(ticks) port = PortHandler(USB_PORT) if not port.openPort(): sys.exit("Failed to open port") if not port.setBaudRate(BAUD_RATE): sys.exit("Failed to set baud rate") sts = sms_sts(port) try: print("Pinging motors...") present_ticks = [] for mid in args.motor_ids: _, comm, err = sts.ping(mid) if comm != 0 or err != 0: sys.exit(f"Motor {mid} not responding") pos, _, _ = sts.ReadPos(mid) present_ticks.append(pos) print(f" motor {mid}: pos={pos} temp={read_temp(sts, mid)}C " f"volt={read_voltage(sts, mid):.1f}V") print("\nPlanned moves:") for mid, curr, goal in zip(args.motor_ids, present_ticks, goals_ticks): print(f" motor {mid}: {curr:>5} -> {goal:>5} (delta {goal - curr:+5})") # Safety handshake for mid, curr in zip(args.motor_ids, present_ticks): sts.WritePosEx(mid, int(curr), args.speed, args.accel) time.sleep(0.2) print(f"\nCommanding goals (speed={args.speed}, accel={args.accel})...") for mid, goal in zip(args.motor_ids, goals_ticks): sts.WritePosEx(mid, int(goal), args.speed, args.accel) # --- Wait for all motors to report stopped print("Waiting for ReadMoving to clear on all motors...") settle_deadline = time.time() + args.settle_timeout while time.time() < settle_deadline: if all(sts.ReadMoving(mid)[0] == 0 for mid in args.motor_ids): break time.sleep(0.05) else: print(" (timed out waiting for motors to stop — sampling anyway)") print("All motors report stopped. Starting sample window.\n") # --- Sample for --hold-time seconds at whatever rate serial allows. # (6 reads/motor * N motors at 115200 typically caps well below 50Hz.) period = 1.0 / args.rate series = {mid: {"pos": [], "speed": [], "load": [], "current": [], "voltage": [], "moving": []} for mid in args.motor_ids} t_start = time.perf_counter() deadline = t_start + args.hold_time while time.perf_counter() < deadline: t_loop = time.perf_counter() for mid in args.motor_ids: pos, _, _ = sts.ReadPos(mid) mov, _, _ = sts.ReadMoving(mid) series[mid]["pos"].append(pos) series[mid]["moving"].append(int(bool(mov))) if args.fast: series[mid]["speed"].append(0) series[mid]["load"].append(0) series[mid]["current"].append(0.0) series[mid]["voltage"].append(0.0) else: spd = read_speed(sts, mid) load = read_signed_load(sts, mid) cur = read_current_ma(sts, mid) volt = read_voltage(sts, mid) series[mid]["speed"].append(spd if spd is not None else 0) series[mid]["load"].append(load if load is not None else 0) series[mid]["current"].append(cur if cur is not None else 0.0) series[mid]["voltage"].append(volt if volt is not None else 0.0) dt = time.perf_counter() - t_loop if dt < period: time.sleep(period - dt) t_end = time.perf_counter() n_samples = len(series[args.motor_ids[0]]["pos"]) actual_rate = n_samples / (t_end - t_start) print(f"Captured {n_samples} samples in {t_end - t_start:.2f}s " f"(effective rate {actual_rate:.1f} Hz)\n") # --- Time-series tail (last 20 samples) tail = min(20, n_samples) print(f"Last {tail} samples (pos ticks, speed ticks/s, load, mov):") header = " ".join( f"[{mid:>2}] pos spd load mov" for mid in args.motor_ids ) print(header) for i in range(n_samples - tail, n_samples): row = [] for mid in args.motor_ids: s = series[mid] row.append( f"[{mid:>2}] {s['pos'][i]:5d} {s['speed'][i]:+5d} " f"{s['load'][i]:+5d} {s['moving'][i]}" ) print(" ".join(row)) # --- Per-motor summary print("\nPer-motor wobble summary over the hold window:") print(f" {'motor':>5} {'goal':>5} {'pos_mean':>9} {'pos_p2p':>8} " f"{'pos_std':>8} {'|spd|max':>9} {'|load|max':>10} " f"{'|cur|max_mA':>12} {'mov_frac':>9} {'Vmin':>5}") def stats(xs): mean = sum(xs) / len(xs) var = sum((x - mean) ** 2 for x in xs) / len(xs) return mean, math.sqrt(var), min(xs), max(xs) worst = None for mid, goal in zip(args.motor_ids, goals_ticks): s = series[mid] pm, ps, pmn, pmx = stats(s["pos"]) sp_max = max(abs(v) for v in s["speed"]) lo_max = max(abs(v) for v in s["load"]) cu_max = max(abs(v) for v in s["current"]) mov_frac = sum(s["moving"]) / len(s["moving"]) v_min = min(s["voltage"]) if s["voltage"] else 0.0 p2p = pmx - pmn print(f" {mid:>5} {goal:>5} {pm:>9.1f} {p2p:>8d} " f"{ps:>8.2f} {sp_max:>9d} {lo_max:>10d} " f"{cu_max:>12.0f} {mov_frac:>9.2f} {v_min:>4.1f}V") if worst is None or p2p > worst[1]: worst = (mid, p2p) print(f"\nPrime suspect: motor {worst[0]} (pos peak-to-peak = {worst[1]} ticks " f"~ {worst[1] * TICKS_TO_RAD * 180 / math.pi:.2f} deg)") print("\nReading hint:") print(" - |load|max non-zero + |spd|max non-zero -> PID hunting at rest") print(" - |load|max ~0 + pos_p2p big -> external / gravity / backlash, not PID") print(" - mov_frac ~ 1 -> servo never clears its own moving bit (too-tight dead zone)") print(" - Vmin significantly below supply -> bus droop under stall") try: if input("\nRelease torque? [y/N] ").strip().lower() == "y": emergency_stop(sts, args.motor_ids) print("Torque disabled on all motors.") else: print("Leaving torque enabled.") except EOFError: pass except KeyboardInterrupt: print("\n[INTERRUPTED] disabling torque.") emergency_stop(sts, args.motor_ids) except Exception: emergency_stop(sts, args.motor_ids) raise finally: port.closePort() if __name__ == "__main__": main()