step.motion.goto

Closed-loop goto motion step — drive to an ABSOLUTE world pose.

goto(x, y, theta) is a navigate-to-pose primitive: each control tick it reads the live pose estimate from the localization particle filter (robot.localization.get_pose()), computes the world-frame error to the target, rotates the translational error into the robot body frame using the current heading, and commands robot.drive a proportional velocity toward the goal (simultaneously correcting heading when a target theta is given). This is fundamentally different from the dead-reckoning relative drives (drive_forward etc.): it regulates on absolute feedback, so it converges on the target pose regardless of accumulated odometry drift.

The proportional control law lives in _compute_body_velocity(), a small pure helper that takes plain numbers / a tiny pose-like duck type so the math is unit-testable without a robot or C++ types. The real step feeds it the genuine localization Pose.

Classes

Goto	Internal closed-loop navigate-to-pose step — users go through goto().
GotoRelative	Internal closed-loop navigate-to-RELATIVE-pose step.
GotoWaypoints	Internal closed-loop navigate through a sequence of ABSOLUTE waypoints.
AbsoluteHoldMove	Closed-loop-on-localization single-axis drive that holds 2 world DOF.
SplineFollow	Continuous pure-pursuit follower of a Catmull-Rom curve, closed-loop.

Functions

goto_relative(→ GotoRelative)	Drive to a pose given as a body-frame delta from the run-start pose.
goto(→ Goto)	Drive to an ABSOLUTE world pose using localization as feedback.

Module Contents

class step.motion.goto.Goto(x_m: float, y_m: float, theta_rad: float | None = None, speed: float = 1.0, pos_tol_m: float = 0.02, heading_tol_rad: float = math.radians(3.0))

Bases: step.motion.motion_step.MotionStep

Internal closed-loop navigate-to-pose step — users go through goto().

Reads the particle-filter pose each tick and commands a proportional body-frame velocity toward the absolute target (x_m, y_m, theta_rad). Requires robot.localization (the particle filter); raises at start otherwise.

required_resources() → frozenset[str]

Return the hardware resources this step requires exclusive access to.

For leaf steps (drive, motor, servo), return the resources this step directly uses. Composite steps override collected_resources instead to include children — required_resources stays empty for composites because they don’t touch hardware themselves.

on_start(robot: raccoon.robot.api.GenericRobot) → None

Called once before the loop. Override to set up motion/velocity.

on_update(robot: raccoon.robot.api.GenericRobot, dt: float) → bool

Called each cycle with dt in seconds. Return True when motion is complete.

class step.motion.goto.GotoRelative(forward_m: float, left_m: float, dtheta_rad: float | None = None, speed: float = 1.0, pos_tol_m: float = 0.02, heading_tol_rad: float = math.radians(3.0))

Bases: step.motion.motion_step.MotionStep

Internal closed-loop navigate-to-RELATIVE-pose step.

Captures the localization pose at on_start as an anchor, composes the body-frame delta (forward_m, left_m, dtheta_rad) onto it to obtain an absolute world target, then runs the IDENTICAL proportional closed-loop as Goto (reusing _compute_body_velocity()). Used by the to_absolute optimizer pass to turn dead-reckoning relative legs into feedback-regulated navigate-to-pose moves whose absolute target is unknown at compile time but fixed at run time relative to the run-start pose.

Requires robot.localization (the particle filter); raises at start otherwise.

required_resources() → frozenset[str]

Return the hardware resources this step requires exclusive access to.

For leaf steps (drive, motor, servo), return the resources this step directly uses. Composite steps override collected_resources instead to include children — required_resources stays empty for composites because they don’t touch hardware themselves.

on_start(robot: raccoon.robot.api.GenericRobot) → None

Called once before the loop. Override to set up motion/velocity.

on_update(robot: raccoon.robot.api.GenericRobot, dt: float) → bool

Called each cycle with dt in seconds. Return True when motion is complete.

class step.motion.goto.GotoWaypoints(waypoints: list[tuple[float, float, float, float, str, float]], speed: float = 1.0, pos_tol_m: float = 0.02, heading_tol_rad: float = math.radians(3.0))

Bases: step.motion.motion_step.MotionStep

Internal closed-loop navigate through a sequence of ABSOLUTE waypoints.

Holds a list of body-frame waypoints [(forward_m, left_m, dtheta_rad|None), ...] expressed in the run-start frame (all relative to ONE anchor — the pose captured at on_start — not chained leg-to-leg). At on_start it captures the localization pose ONCE and precomputes the ABSOLUTE world target for every waypoint via _waypoints_to_world_targets(). It then runs the IDENTICAL proportional closed-loop as Goto / GotoRelative (reusing _compute_body_velocity()) toward the current target; when that target is reached it advances to the next, and the step finishes once the LAST target is reached.

Because all targets are fixed at run start relative to a single anchor, leg N drives to a fixed world point — accumulated drift does NOT chain from one leg to the next (unlike emitting one re-anchoring GotoRelative per leg). Used by the to_absolute optimizer pass: it collects a whole contiguous run’s relative waypoints and emits ONE GotoWaypoints.

Requires robot.localization (the particle filter); raises at start otherwise.

required_resources() → frozenset[str]

Return the hardware resources this step requires exclusive access to.

For leaf steps (drive, motor, servo), return the resources this step directly uses. Composite steps override collected_resources instead to include children — required_resources stays empty for composites because they don’t touch hardware themselves.

on_start(robot: raccoon.robot.api.GenericRobot) → None

Called once before the loop. Override to set up motion/velocity.

on_update(robot: raccoon.robot.api.GenericRobot, dt: float) → bool

Called each cycle with dt in seconds. Return True when motion is complete.

class step.motion.goto.AbsoluteHoldMove(free_axis: raccoon.motion.LinearAxis, sign: float, speed: float, condition: raccoon.step.condition.StopCondition, hold_heading_rad: float | None = None)

Bases: step.motion.motion_step.MotionStep

Closed-loop-on-localization single-axis drive that holds 2 world DOF.

A SENSOR-bounded single-axis drive (e.g. strafe_left().until(on_black)) pins 2 of 3 world DOF — the cross-axis position and the heading — and leaves only the travel (free) axis free until the sensor fires. This step drives the free body-axis open-loop at speed while CORRECTING drift on the two pinned DOF against their absolute (localization) targets DURING the move, until the original .until() condition fires.

At on_start it captures the localization pose as an anchor. The pinned target is: stay on the world line through anchor_pos along the free-axis direction (cross-axis displacement = 0) and hold anchor_heading. Each tick it reads the live pose and feeds _hold_velocity() (read-only on localization — it never writes the filter). The result is the whole to_absolute path regulating on the particle filter instead of odometry dead reckoning.

Requires robot.localization (the particle filter); raises at start otherwise.

required_resources() → frozenset[str]

Return the hardware resources this step requires exclusive access to.

For leaf steps (drive, motor, servo), return the resources this step directly uses. Composite steps override collected_resources instead to include children — required_resources stays empty for composites because they don’t touch hardware themselves.

on_start(robot: raccoon.robot.api.GenericRobot) → None

Called once before the loop. Override to set up motion/velocity.

on_update(robot: raccoon.robot.api.GenericRobot, dt: float) → bool

Called each cycle with dt in seconds. Return True when motion is complete.

class step.motion.goto.SplineFollow(waypoints: list[tuple[float, float]], speed: float = 1.0, heading_mode: str | float = _HEADING_HOLD, pos_tol_m: float = 0.02, lookahead_m: float = _DEFAULT_LOOKAHEAD_M, spacing_m: float = 0.03)

Bases: step.motion.motion_step.MotionStep

Continuous pure-pursuit follower of a Catmull-Rom curve, closed-loop.

Rides a centripetal Catmull-Rom spline SMOOTHLY on the localization particle filter instead of stop-and-go stepping through dense waypoints. At on_start it captures the live pose as an anchor, transforms the body-frame control points (forward_m, left_m) (run-start frame, same format as GotoWaypoints) into absolute world points via _anchor_to_world_target(), and densely samples the curve through them (sample_centripetal_catmull_rom()) into a world polyline.

Each tick it PROJECTS the live pose onto the polyline (monotonic forward progress), picks a carrot lookahead_m ahead along the arc, and commands a proportional body velocity toward it (_pursuit_velocity()). HEADING is an INDEPENDENT channel — hold the anchor heading, interpolate to a goal angle with progress, or face the path tangent — so a mecanum base can strafe along the curve without rotating to follow it. It finishes when the projection reaches the end of the polyline and the robot is within pos_tol_m of the final point.

Requires robot.localization (the particle filter); raises at start otherwise.

Parameters:

• waypoints – Body-frame control points [(forward_m, left_m), ...] in the run-start frame (relative to the single on_start anchor).

• speed – Velocity scale in (0.0, 1.0] applied to the pursuit command.

• heading_mode – "hold" (default — keep the anchor heading), "tangent" (face the local curve tangent), or a float goal angle in radians (interpolate from the anchor heading to it with progress).

• pos_tol_m – Final-point position tolerance (metres) for completion.

• lookahead_m – Pure-pursuit lookahead arc length (metres, default 0.15).

• spacing_m – Dense-sample spacing along the spline (metres, default 0.03).

required_resources() → frozenset[str]

Return the hardware resources this step requires exclusive access to.

For leaf steps (drive, motor, servo), return the resources this step directly uses. Composite steps override collected_resources instead to include children — required_resources stays empty for composites because they don’t touch hardware themselves.

on_start(robot: raccoon.robot.api.GenericRobot) → None

Called once before the loop. Override to set up motion/velocity.

on_update(robot: raccoon.robot.api.GenericRobot, dt: float) → bool

Called each cycle with dt in seconds. Return True when motion is complete.

step.motion.goto.goto_relative(forward_cm: float, left_cm: float = 0.0, dtheta_deg: float | None = None, speed: float = 1.0, pos_tol_cm: float = 2.0, heading_tol_deg: float = 3.0) → GotoRelative

Drive to a pose given as a body-frame delta from the run-start pose.

Closed-loop navigate-to-pose by delta: at start the step captures the live localization pose as an anchor, composes the body-frame displacement (forward_cm, left_cm) and optional heading change dtheta_deg onto it to obtain an absolute world target, then regulates onto that target with the IDENTICAL proportional controller as goto() (reading robot.localization.get_pose() each tick). Unlike the dead-reckoning drive_forward / strafe_* steps, it converges on the computed target pose regardless of accumulated odometry drift.

The absolute target is not known at construction time — it equals anchor ⊕ delta and is fixed only once the anchor pose is sampled at on_start. This is what lets the to_absolute optimizer pass convert a run of relative legs into closed-loop moves with no executor changes.

Sign convention (matches goto() / the HAL ChassisVelocity): +forward_cm is the anchor heading direction, +left_cm is 90° CCW (to the robot’s left), +dtheta_deg is counter-clockwise.

Prerequisites:

• robot.localization (the particle filter) must be available; the step raises RuntimeError at start if it is missing.

• A mecanum / omni-wheel drivetrain for full 2-D translation.

Parameters:

• forward_cm – Body-forward displacement from the anchor, centimetres.

• left_cm – Body-left displacement from the anchor, centimetres (default 0.0).

• dtheta_deg – Heading change relative to the anchor heading, degrees, or None (default) to leave heading uncorrected.

• speed – Velocity scale in (0.0, 1.0] (default 1.0).

• pos_tol_cm – Position tolerance in centimetres (default 2.0).

• heading_tol_deg – Heading tolerance in degrees (default 3.0); only used when dtheta_deg is given.

Returns:

GotoRelative — a MotionStep running the closed-loop controller against the anchor-relative target.

Raises:

RuntimeError – at start, if robot.localization is unavailable.

Example:

from raccoon.step.motion import goto_relative

goto_relative(50)  # drive 0.5 m forward (closed-loop)
goto_relative(50, dtheta_deg=90)  # 0.5 m forward, end 90° CCW
goto_relative(30, left_cm=20)  # 0.3 m forward + 0.2 m left

step.motion.goto.goto(x_cm: float, y_cm: float, theta_deg: float | None = None, speed: float = 1.0, pos_tol_cm: float = 2.0, heading_tol_deg: float = 3.0) → Goto

Drive to an ABSOLUTE world pose using localization as feedback.

Closed-loop navigate-to-pose primitive. Each control tick the step reads the live pose estimate from the localization particle filter (robot.localization.get_pose()), computes the world-frame error to the target (x_cm, y_cm), rotates that translational error into the robot body frame using the current heading, and commands robot.drive a proportional velocity toward the goal. When theta_deg is given the step simultaneously rotates toward that absolute heading; otherwise heading is left uncorrected. The step finishes once the robot is within pos_tol_cm of the target AND (within heading_tol_deg of theta_deg, or no heading was requested).

Because it regulates on the absolute particle-filter estimate rather than dead reckoning, goto converges on the target pose regardless of accumulated odometry drift — unlike the relative drive_forward / strafe_* steps. It commands lateral velocity, so it needs an omni-directional (mecanum / omni-wheel) drivetrain to translate freely; on a differential base only the forward and heading axes are effective.

Prerequisites:

• robot.localization (the particle filter) must be available; anchor it first (e.g. resync_at_start_pose()). The step raises RuntimeError at start if localization is missing.

• A mecanum / omni-wheel drivetrain for full 2-D translation.

Parameters:

• x_cm – Target world X position, centimetres.

• y_cm – Target world Y position, centimetres.

• theta_deg – Target absolute heading in degrees, or None (default) to leave heading uncorrected.

• speed – Velocity scale in (0.0, 1.0] (default 1.0) applied to the proportional command before per-axis saturation.

• pos_tol_cm – Position tolerance in centimetres (default 2.0).

• heading_tol_deg – Heading tolerance in degrees (default 3.0); only used when theta_deg is given.

Returns:

Goto — a MotionStep running the closed-loop controller.

Raises:

RuntimeError – at start, if robot.localization is unavailable.

Example:

from raccoon.step.motion import goto, resync_at_start_pose

resync_at_start_pose(expected_x_cm=0, expected_y_cm=0, expected_theta_deg=0)
goto(100, 50, theta_deg=90)  # drive to (1.0 m, 0.5 m), face 90°
goto(30, 0)  # drive to (0.3 m, 0 m), hold heading
goto(50, 50, speed=0.5, pos_tol_cm=1.0)