step.motion.goto ================ .. py:module:: step.motion.goto .. autoapi-nested-parse:: 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 :func:`_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 ------- .. autoapisummary:: step.motion.goto.Goto step.motion.goto.GotoRelative step.motion.goto.GotoWaypoints step.motion.goto.AbsoluteHoldMove step.motion.goto.SplineFollow Functions --------- .. autoapisummary:: step.motion.goto.goto_relative step.motion.goto.goto Module Contents --------------- .. py:class:: 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: :py:obj:`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. .. py:method:: 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. .. py:method:: on_start(robot: raccoon.robot.api.GenericRobot) -> None Called once before the loop. Override to set up motion/velocity. .. py:method:: on_update(robot: raccoon.robot.api.GenericRobot, dt: float) -> bool Called each cycle with dt in seconds. Return True when motion is complete. .. py:class:: 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: :py:obj:`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 :class:`Goto` (reusing :func:`_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. .. py:method:: 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. .. py:method:: on_start(robot: raccoon.robot.api.GenericRobot) -> None Called once before the loop. Override to set up motion/velocity. .. py:method:: on_update(robot: raccoon.robot.api.GenericRobot, dt: float) -> bool Called each cycle with dt in seconds. Return True when motion is complete. .. py:class:: 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: :py:obj:`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 :func:`_waypoints_to_world_targets`. It then runs the IDENTICAL proportional closed-loop as :class:`Goto` / :class:`GotoRelative` (reusing :func:`_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 :class:`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. .. py:method:: 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. .. py:method:: on_start(robot: raccoon.robot.api.GenericRobot) -> None Called once before the loop. Override to set up motion/velocity. .. py:method:: on_update(robot: raccoon.robot.api.GenericRobot, dt: float) -> bool Called each cycle with dt in seconds. Return True when motion is complete. .. py:class:: AbsoluteHoldMove(free_axis: raccoon.motion.LinearAxis, sign: float, speed: float, condition: raccoon.step.condition.StopCondition, hold_heading_rad: float | None = None) Bases: :py:obj:`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 :func:`_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. .. py:method:: 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. .. py:method:: on_start(robot: raccoon.robot.api.GenericRobot) -> None Called once before the loop. Override to set up motion/velocity. .. py:method:: on_update(robot: raccoon.robot.api.GenericRobot, dt: float) -> bool Called each cycle with dt in seconds. Return True when motion is complete. .. py:class:: 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: :py:obj:`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 :class:`GotoWaypoints`) into absolute world points via :func:`_anchor_to_world_target`, and densely samples the curve through them (:func:`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 (:func:`_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. :param waypoints: Body-frame control points ``[(forward_m, left_m), ...]`` in the run-start frame (relative to the single ``on_start`` anchor). :param speed: Velocity scale in ``(0.0, 1.0]`` applied to the pursuit command. :param 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). :param pos_tol_m: Final-point position tolerance (metres) for completion. :param lookahead_m: Pure-pursuit lookahead arc length (metres, default ``0.15``). :param spacing_m: Dense-sample spacing along the spline (metres, default ``0.03``). .. py:method:: 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. .. py:method:: on_start(robot: raccoon.robot.api.GenericRobot) -> None Called once before the loop. Override to set up motion/velocity. .. py:method:: on_update(robot: raccoon.robot.api.GenericRobot, dt: float) -> bool Called each cycle with dt in seconds. Return True when motion is complete. .. py:function:: 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 :func:`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 :func:`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. :param forward_cm: Body-forward displacement from the anchor, centimetres. :param left_cm: Body-left displacement from the anchor, centimetres (default ``0.0``). :param dtheta_deg: Heading change relative to the anchor heading, degrees, or ``None`` (default) to leave heading uncorrected. :param speed: Velocity scale in ``(0.0, 1.0]`` (default ``1.0``). :param pos_tol_cm: Position tolerance in centimetres (default ``2.0``). :param heading_tol_deg: Heading tolerance in degrees (default ``3.0``); only used when ``dtheta_deg`` is given. :returns: :class:`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 .. py:function:: 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. :param x_cm: Target world X position, centimetres. :param y_cm: Target world Y position, centimetres. :param theta_deg: Target absolute heading in degrees, or ``None`` (default) to leave heading uncorrected. :param speed: Velocity scale in ``(0.0, 1.0]`` (default ``1.0``) applied to the proportional command before per-axis saturation. :param pos_tol_cm: Position tolerance in centimetres (default ``2.0``). :param heading_tol_deg: Heading tolerance in degrees (default ``3.0``); only used when ``theta_deg`` is given. :returns: :class:`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)