MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
arm adapter: torque on only after every servo's goal register is latched to its present position (a servo enables onto the goal its register holds and would jump to a stale one); bench checks the write order
metrox2a5c0da1ed02
diff --git a/benches/bench_moss_arm_feetech_cold.py b/benches/bench_moss_arm_feetech_cold.pyindex 9c4c897..39deaa7 100644--- a/benches/bench_moss_arm_feetech_cold.py+++ b/benches/bench_moss_arm_feetech_cold.py@@ -9,8 +9,10 @@ Known input -> known output in physical units. Groups:B. connect - the adapter opens the fake bus, reads the six servos, setsspeed 400 and acceleration 50 on the five arm servos, theforce 400 on the gripper's registers 44 and 48, writes no- torque; activate turns torque on and holds the present- position; limits = the calibration's end stops in radians+ torque; activate latches every servo's goal register to its+ present position (six writes of register 42, all before the+ first torque write) then turns torque on; limits = the+ calibration's end stops in radiansC. follow - a goal of +30 deg on the pan writes 2389 to register 42 ofservo 1 and the fake servo reaches it at 400 ticks/s: readback +30.0 deg; the gripper to 53 mm writes 2780 and reads@@ -186,8 +188,16 @@ check(ad.present_ticks() == {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_q = ad.read_joint_positions()check(abs(q[0] * DEG - 71.1) < 0.05 and abs(q[5] - 0.090) < 1e-9, f"positions: pan {q[0] * DEG:.1f} deg, gap {q[5] * 1000:.0f} mm")check(not ad.read_enabled() and ad.write_joint_positions(q) is False, "no command before activate")+n0 = len(bus.writes)check(ad.activate() and ad.read_enabled() and all(s.torque == 1 for s in bus.servos.values()), "activate: torque on, six servos")-check(ad.goal_ticks() == ad.present_ticks(), "torque on holds the present position")+w = bus.writes[n0:]+goals = [(sid, val) for sid, addr, val in w if addr == F.REG_GOAL]+first_torque = next((i for i, (sid, addr, val) in enumerate(w) if addr == F.REG_TORQUE), None)+last_goal = max((i for i, (sid, addr, val) in enumerate(w) if addr == F.REG_GOAL), default=None)+check(len(goals) == 6 and all(val == bus.servos[sid].goal == int(round(bus.servos[sid].pos)) for sid, val in goals),+ "before torque: every servo's goal register latched to its present position (six writes of register 42)")+check(first_torque is not None and last_goal is not None and last_goal < first_torque, "all six goal writes come before the first torque write")+check(ad.goal_ticks() == ad.present_ticks(), "the host's goal cache holds the present position too")lim = ad.get_limits()check(abs(lim.position_lower[1] * DEG + 104.4) < 0.05 and abs(lim.position_upper[0] * DEG - 118.3) < 0.05 and lim.position_upper[5] == 0.090,f"limits: lift from {lim.position_lower[1] * DEG:.1f} deg, pan to {lim.position_upper[0] * DEG:.1f} deg, gap to 90 mm")diff --git a/moss_dimos/arm_feetech.py b/moss_dimos/arm_feetech.pyindex 90bfa93..62eac52 100644--- a/moss_dimos/arm_feetech.py+++ b/moss_dimos/arm_feetech.py@@ -13,7 +13,9 @@ Per coordinator tick: one read of every servo's present position (register56), then one goal write (register 42) for every servo whose target movedby a tick or more. Goal speed (46) and acceleration (41) are set once atconnect: the servo's own ceiling, so a streamed position cannot ask more-than the sweep's speed. Torque (40) goes on at activate and off at+than the sweep's speed. Torque (40) goes on at activate, after every goal+register has been set to the present position (a servo enables onto the+goal its register holds, and would jump to a stale one), and off atdeactivate; the gripper's force limit goes to registers 44 and 48 atconnect.@@ -401,15 +403,29 @@ class FeetechSo101Adapter:return okdef write_enable(self, enable: bool) -> bool:+ """Torque on or off on the six servos. Before torque goes on, every+ servo's goal register is set to its present position: a servo+ enables onto whatever goal its register holds (the last run's,+ after a power cycle anything), and the arm would jump there."""if not self._connected:return Falseok = True+ if enable:+ if self._read_all() is None:+ self._error = (BUS_ERROR, "the servos do not answer before torque on")+ return False+ for j in self._joints():+ if self._bus.write2(self._ids[j], REG_GOAL, self._present[j]):+ self._goal[j] = self._present[j]+ else:+ ok = False+ if not ok:+ self._error = (BUS_ERROR, "a goal could not be latched before torque on; torque stays off")+ return Falsefor j in self._joints():ok &= self._bus.write1(self._ids[j], REG_TORQUE, 1 if enable else 0)if ok:self._enabled = bool(enable)- if enable:- self._goal = dict(self._present) # torque on: hold where the arm isself._lag_since.clear()return ok