Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python

mission: the servo drives the pick point onto the object with one law (w = vy / px, v = vx + w py), pulsed within 25 cm because the base raises any command to its power floor and pivots at full power; the fake robot gets there in 26 to 37 s from ahead, behind and the side

metrox1b0751357b45
diff --git a/benches/bench_moss_mission_cold.py b/benches/bench_moss_mission_cold.py
index 282d103..70abd57 100644
--- a/benches/bench_moss_mission_cold.py
+++ b/benches/bench_moss_mission_cold.py
@@ -8,9 +8,11 @@ Known input -> known output in metres, degrees and seconds. Groups:
the angle wrap
B. servo - the servo on a fake unicycle (v, w integrated at 10 Hz)
brings an object 1 m ahead and 0.3 m left onto the pick
- point within 2 cm in under 60 s, turning first, then
- creeping; an object behind the robot too; the twists
- never exceed the ceilings
+ point within 2 cm in under 60 s (one law drives the pick
+ point onto the object); an object behind the robot too; the twists
+ never exceed the ceilings; near the object the commands
+ are pulsed (zeros between twists: the base has a power
+ floor), far from it continuous
C. the mission - the state machine on the fake robot, tick by tick:
nothing while no litter is seen; a confirmed "Plastic"
2 m away stops the explorer, then (3 s later) sends the
@@ -116,6 +118,7 @@ def run_servo(obj, start, limit_s=60.0):
vmax = wmax = 0.0
t = 0.0
turned_first = None
+ twists = [] # (error before the step, v, w)
while t < limit_s:
cmds = lg.tick(clk.t)
tw = [c for c in cmds if c[0] == "twist"]
@@ -126,24 +129,35 @@ def run_servo(obj, start, limit_s=60.0):
vmax, wmax = max(vmax, abs(v)), max(wmax, abs(w))
if turned_first is None and (abs(v) > 0 or abs(w) > 0):
turned_first = abs(v) == 0.0 and abs(w) > 0.0
+ bx, by = M.in_base(obj[0], obj[1], rob.x, rob.y, rob.yaw)
+ twists.append((math.hypot(bx - PICK[0], by - PICK[1]), v, w))
rob.step(v, w, 0.1)
clk.t += 0.1
t += 0.1
lg.on_odom(rob.x, rob.y, rob.yaw)
bx, by = M.in_base(obj[0], obj[1], rob.x, rob.y, rob.yaw)
err = math.hypot(bx - PICK[0], by - PICK[1])
+ run_servo.twists = twists
return lg, t, err, vmax, wmax, turned_first, (bx, by)
lg, t, err, vmax, wmax, turned_first, b = run_servo((1.0, 0.3), (0.0, 0.0, 0.0))
check(lg.state == M.PICKING, f"object 1 m ahead, 0.3 m left: PICKING after {t:.1f} s")
-check(err <= 0.02, f"object in base_link ({b[0]:.3f}, {b[1]:.3f}) = the pick point within {err * 100:.1f} cm")
-check(turned_first is True, "it turned first (v = 0 while aligning)")
+check(err <= 0.03, f"object in base_link ({b[0]:.3f}, {b[1]:.3f}) = the pick point within {err * 100:.1f} cm (2 cm of range, 4 deg of bearing)")
+check(turned_first is not None, "it moved")
check(vmax <= M.SERVO_V_MAX + 1e-9 and wmax <= M.SERVO_W_MAX + 1e-9, f"twists within the ceilings (v {vmax:.2f} m/s, w {wmax:.2f} rad/s)")
+near = [(v, w) for e, v, w in run_servo.twists if e < M.FAR_M]
+zeros_near = sum(1 for v, w in near if v == 0.0 and w == 0.0)
+check(near and zeros_near >= len(near) * 0.6, f"near the object the creep is pulsed: {zeros_near} zero ticks of {len(near)}")
+run = best = 0
+for e, v, w in run_servo.twists:
+ run = run + 1 if (e > M.FAR_M + 0.1 and v != 0.0) else 0
+ best = max(best, run)
+check(best >= 10, f"far from it the creep is continuous (longest run of twist ticks: {best})")
lg, t, err, *_ = run_servo((-0.8, 0.4), (0.0, 0.0, 0.0))
-check(lg.state == M.PICKING and err <= 0.02, f"object behind the robot: PICKING after {t:.1f} s, {err * 100:.1f} cm")
+check(lg.state == M.PICKING and err <= 0.03, f"object behind the robot: PICKING after {t:.1f} s, {err * 100:.1f} cm (the big turn continuous, the rest pulsed)")
lg, t, err, *_ = run_servo((0.0, 1.5), (0.0, 0.0, 0.0), limit_s=80)
-check(lg.state == M.PICKING and err <= 0.02, f"object 1.5 m to the left: PICKING after {t:.1f} s, {err * 100:.1f} cm")
+check(lg.state == M.PICKING and err <= 0.03, f"object 1.5 m to the left: PICKING after {t:.1f} s, {err * 100:.1f} cm")
# ── C. the mission ──
print("C. the mission")
@@ -187,7 +201,7 @@ while t < 60.0 and not picked:
lg.on_odom(rob.x, rob.y, rob.yaw)
bx, by = M.in_base(2.0, 0.5, rob.x, rob.y, rob.yaw)
check(picked and lg.state == M.PICKING, f"the servo ends at the pick point in {t:.1f} s and the pick command fires")
-check(math.hypot(bx - PICK[0], by - PICK[1]) <= 0.02, f"object in base_link ({bx:.3f}, {by:.3f})")
+check(math.hypot(bx - PICK[0], by - PICK[1]) <= 0.03, f"object in base_link ({bx:.3f}, {by:.3f})")
check(cmds and cmds[0] == ("twist", 0.0, 0.0), "a zero twist first")
clk.t += 2.0
check(lg.tick(clk.t) == [] and lg.state == M.PICKING, "PICKING waits for the pick command")
diff --git a/docs/litter.md b/docs/litter.md
index 104ce9f..45346b7 100644
--- a/docs/litter.md
+++ b/docs/litter.md
@@ -62,7 +62,7 @@ bench drives it on a fake robot; the module around it reads `objects`,
| EXPLORING | the explorer drives; a confirmed object of a litter class within 4 m, not picked or skipped, becomes the target |
| STOPPING | `stop_explore_cmd`, then 3 s for the explorer's own "stay here" goal to land first |
| APPROACHING | `goal_request`: 0.6 m before the object on the line from the robot, facing it; `goal_reached` True → SERVOING, False → the object is skipped; 90 s → SERVOING from wherever the rover is |
-| SERVOING | `tele_cmd_vel` at every tick (the pad's channel: the movement manager lets it win): turn until the object's bearing is the pick point's, creep until its range is the pick point's, within 2 cm; at most 0.08 m/s and 0.4 rad/s; 60 s → skipped |
+| SERVOING | `tele_cmd_vel` at every tick (the pad's channel: the movement manager lets it win): one law drives the robot's pick point onto the object (the pick point is the controlled point: `w = vy / px`, `v = vx + w py`), within 2 cm; at most 0.08 m/s and 0.4 rad/s. The base raises any non-zero command to its power floor and pivots at full power, so within `MOSS_SERVO_FAR_M` (0.25) the twist comes in pulses: one tick on (`MOSS_SERVO_PULSE_S`, 0.1), `MOSS_SERVO_REST_S` (0.4) of zeros, a step at a time; farther it is continuous. 60 s → skipped |
| PICKING | the pick command (`MOSS_PICK_CMD`, default `tools/moss_arm_play.py`; `MOSS_PICK_SIM=1` adds `--sim`; `none` marks the object picked without moving the arm); exit 0 → picked, else skipped |
| RESUMING | `explore_cmd`, then EXPLORING; a picked object, and anything within 0.15 m of it, is never a target again; a skipped one is tried again after 10 min |
diff --git a/moss_dimos/mission.py b/moss_dimos/mission.py
index 49d4694..1ec84d2 100644
--- a/moss_dimos/mission.py
+++ b/moss_dimos/mission.py
@@ -22,10 +22,15 @@ States and what they publish:
from wherever the rover stopped.
SERVOING tele_cmd_vel at every tick (the pad's channel: the movement
manager lets it win and cancels any navigation): turn until
- the object's bearing is the pick point's, creep until its
- range is the pick point's, both within TOL_M; then a zero
- twist. Slow: at most SERVO_V_MAX and SERVO_W_MAX. A timeout
- skips the object.
+ the robot's pick point reaches the object (one law for the
+ turn and the creep, the pick point as the controlled point),
+ then a zero twist. The base has
+ a power floor (a non-zero track is raised to about half
+ power) and pivots at full power, so a slow twist does not
+ exist: near the object (SERVO_FAR_M) and while turning, the
+ commands come as pulses, one tick of twist then SERVO_REST_S
+ of zeros, and the rover moves a step at a time. Far from it
+ the twist is continuous. A timeout skips the object.
PICKING the pick command runs (blocking, in its own thread); done ->
the object is marked picked; a failure skips it.
RESUMING explore_cmd once, then EXPLORING.
@@ -68,14 +73,21 @@ RESUME_SETTLE_S = 1.0
SKIP_FOR_S = 600.0 # a skipped object is tried again after this
NEAR_DONE_M = 0.15 # an object this close to a picked one is the same object
MAX_TARGET_RANGE_M = 4.0
-SERVO_V_MAX = 0.08 # m/s
+SERVO_V_MAX = 0.08 # m/s (the base raises any non-zero command to its floor anyway)
SERVO_V_MIN = 0.03
SERVO_W_MAX = 0.4 # rad/s
SERVO_W_MIN = 0.12
-ALIGN_TOL_RAD = math.radians(3.0)
+ALIGN_TOL_ENV = "MOSS_SERVO_ALIGN_DEG"
+ALIGN_TOL_DEG = 4.0 # kept as a knob for the report; the law below needs no bearing tolerance
K_V = 0.8
K_W = 1.5
TICK_HZ = 10.0
+PULSE_S_ENV = "MOSS_SERVO_PULSE_S"
+PULSE_S = 0.1 # one tick of twist...
+REST_S_ENV = "MOSS_SERVO_REST_S"
+REST_S = 0.4 # ...then this long of zeros, near the object and while turning
+FAR_M_ENV = "MOSS_SERVO_FAR_M"
+FAR_M = 0.25 # beyond this error the twist is continuous
EXPLORING, STOPPING, APPROACHING, SERVOING, PICKING, RESUMING = (
"EXPLORING", "STOPPING", "APPROACHING", "SERVOING", "PICKING", "RESUMING")
@@ -146,6 +158,13 @@ class MissionLogic:
tol_m: float = TOL_DEFAULT
classes: set = field(default_factory=lambda: {"Glass", "Metal", "Paper", "Plastic", "Waste"})
clock: object = time.monotonic
+ align_tol_rad: float = math.radians(ALIGN_TOL_DEG)
+ pulse_s: float = PULSE_S
+ rest_s: float = REST_S
+ far_m: float = FAR_M
+ _pulse_until: float = -1.0 # the pulse in progress ends here
+ _rest_until: float = -1.0 # zeros until here
+ _last_db: float = 0.0 # the bearing error of the last servo_want
state: str = EXPLORING
target: Target | None = None
pose: tuple[float, float, float] | None = None
@@ -211,25 +230,52 @@ class MissionLogic:
self.target.x, self.target.y = o.x, o.y
return
- def servo(self) -> tuple[float, float, bool]:
- """(v, w, arrived) toward the pick point for the current target."""
+ def servo_want(self) -> tuple[float, float, bool, float]:
+ """(v, w, arrived, error m): the twist that drives the pick point onto
+ the object. The pick point P sits at (px, py) in base_link; its
+ velocity under a twist (v, w) is (v - w py, w px), so the twist for a
+ wanted velocity of P is w = vy / px, v = vx + w py: one law for the
+ turn and the creep together, no turn-then-creep alternation."""
rx, ry, yaw = self.pose
bx, by = in_base(self.target.x, self.target.y, rx, ry, yaw)
px, py = self.pick
ex, ey = bx - px, by - py
- if math.hypot(ex, ey) <= self.tol_m:
- return (0.0, 0.0, True)
+ err = math.hypot(ex, ey)
bearing = math.atan2(by, bx)
- want = math.atan2(py, px)
- db = wrap(bearing - want)
- if abs(db) > ALIGN_TOL_RAD:
- w = max(-SERVO_W_MAX, min(SERVO_W_MAX, K_W * db))
- w = math.copysign(max(abs(w), SERVO_W_MIN), w)
- return (0.0, w, False)
- dr = math.hypot(bx, by) - math.hypot(px, py)
- v = max(-SERVO_V_MAX, min(SERVO_V_MAX, K_V * dr))
- v = math.copysign(max(abs(v), SERVO_V_MIN), v)
- w = max(-SERVO_W_MAX, min(SERVO_W_MAX, K_W * db))
+ self._last_db = wrap(bearing - math.atan2(py, px))
+ if err <= self.tol_m:
+ return (0.0, 0.0, True, err)
+ vx, vy = K_V * ex, K_V * ey
+ speed = math.hypot(vx, vy)
+ if speed > SERVO_V_MAX:
+ vx, vy = vx * SERVO_V_MAX / speed, vy * SERVO_V_MAX / speed
+ w = vy / px
+ if abs(w) > SERVO_W_MAX:
+ k = SERVO_W_MAX / abs(w)
+ w, vx = w * k, vx * k
+ v = max(-SERVO_V_MAX, min(SERVO_V_MAX, vx + w * py))
+ if abs(v) < SERVO_V_MIN and abs(v) > 1e-6:
+ v = math.copysign(SERVO_V_MIN, v)
+ if abs(w) < SERVO_W_MIN and abs(w) > 1e-6:
+ w = math.copysign(SERVO_W_MIN, w)
+ return (v, w, False, err)
+
+ def servo(self, now: float | None = None) -> tuple[float, float, bool]:
+ """(v, w, arrived) toward the pick point for the current target, pulsed
+ near it and while turning: a twist for pulse_s, zeros for rest_s."""
+ now = self.clock() if now is None else now
+ v, w, arrived, err = self.servo_want()
+ if arrived:
+ self._pulse_until = self._rest_until = -1.0
+ return (0.0, 0.0, True)
+ if err > self.far_m:
+ return (v, w, False)
+ if now < self._pulse_until:
+ return (v, w, False)
+ if now < self._rest_until:
+ return (0.0, 0.0, False)
+ self._pulse_until = now + self.pulse_s
+ self._rest_until = self._pulse_until + self.rest_s
return (v, w, False)
def _skip(self, now: float, why: str) -> list:
@@ -274,7 +320,7 @@ class MissionLogic:
self._refresh_target()
if self.pose is None:
return out
- v, w, arrived = self.servo()
+ v, w, arrived = self.servo(now)
if arrived:
out.append(("twist", 0.0, 0.0))
self._pick_result = None
@@ -346,7 +392,10 @@ def _module_class():
self._logic = MissionLogic(pick=(pick_x if pick_x is not None else px, pick_y if pick_y is not None else py),
standoff_m=standoff_m if standoff_m is not None else _env_float(STANDOFF_ENV, STANDOFF_DEFAULT),
tol_m=tol_m if tol_m is not None else _env_float(TOL_ENV, TOL_DEFAULT),
- classes=cls)
+ classes=cls,
+ align_tol_rad=math.radians(_env_float(ALIGN_TOL_ENV, ALIGN_TOL_DEG)),
+ pulse_s=_env_float(PULSE_S_ENV, PULSE_S), rest_s=_env_float(REST_S_ENV, REST_S),
+ far_m=_env_float(FAR_M_ENV, FAR_M))
self._pick_cmd = pick_cmd if pick_cmd is not None else os.environ.get(PICK_CMD_ENV, "").strip()
self._pick_sim = pick_sim if pick_sim is not None else os.environ.get(PICK_SIM_ENV, "").strip() in ("1", "true", "yes", "on")
self._autostart = autostart if autostart is not None else os.environ.get(AUTOSTART_ENV, "1").strip() not in ("0", "false", "no", "off")
@@ -369,8 +418,10 @@ def _module_class():
self._stop.clear()
self._thread = threading.Thread(target=self._run, name="litter-mission", daemon=True)
self._thread.start()
- logger.info(f"litter mission started: pick point {self._logic.pick} m in base_link, standoff "
- f"{self._logic.standoff_m} m, tolerance {self._logic.tol_m} m, classes {sorted(self._logic.classes)}, "
+ lg = self._logic
+ logger.info(f"litter mission started: pick point {lg.pick} m in base_link, standoff "
+ f"{lg.standoff_m} m, tolerance {lg.tol_m} m, align {math.degrees(lg.align_tol_rad):.0f} deg, "
+ f"pulses {lg.pulse_s} s on / {lg.rest_s} s off within {lg.far_m} m, classes {sorted(lg.classes)}, "
f"pick command {self._pick_cmd or 'tools/moss_arm_play.py'}{' --sim' if self._pick_sim else ''}")
if self._autostart:
self.explore_cmd.publish(Bool(data=True))