From 0354eb46757149ebea6ed774f285755fa5c6a27f Mon Sep 17 00:00:00 2001 From: theo-michel Date: Thu, 9 Jul 2026 20:07:21 -0700 Subject: [PATCH 1/5] docs(skills): odometry robot state is now a typed 2D pose Matches innate-inc/innate-os#516: LAST_ODOM now injects innate.Odometry (x, y, theta + velocities) instead of a raw-message dict with a quaternion. Documents the attribute table and the deprecated dict-style access, and fixes the MonitorPosition example. --- snippets/interface-methods-table.mdx | 4 +-- .../code-defined-skills/robot-state.mdx | 35 +++++++++++++------ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/snippets/interface-methods-table.mdx b/snippets/interface-methods-table.mdx index c9930cb..f9f9c67 100644 --- a/snippets/interface-methods-table.mdx +++ b/snippets/interface-methods-table.mdx @@ -438,7 +438,7 @@ export const RobotStateAvailableTable = () => { { state: "Odometry", typeEnum: "RobotStateType.LAST_ODOM", - description: "Position, orientation, and velocity.", + description: "2D pose (x, y, theta) and velocities.", }, { state: "Map", @@ -540,7 +540,7 @@ export const DigitalStateTypesTable = () => { }, { stateType: "LAST_ODOM", - description: "Current odometry.", + description: "2D pose (x, y, theta) and velocities.", }, { stateType: "LAST_MAP", diff --git a/software/skills/code-defined-skills/robot-state.mdx b/software/skills/code-defined-skills/robot-state.mdx index c01b2b7..b2f3eec 100644 --- a/software/skills/code-defined-skills/robot-state.mdx +++ b/software/skills/code-defined-skills/robot-state.mdx @@ -51,7 +51,9 @@ class MySkill(Skill): ## Odometry -Current position, orientation, and velocity: +Where the robot is and how it is moving, as an `innate.Odometry` object. MARS +is a differential-drive base on flat ground, so you get a flat 2D pose — +`x`, `y`, and a yaw angle — directly, no quaternion math needed: ```python class MySkill(Skill): @@ -59,13 +61,27 @@ class MySkill(Skill): def execute(self): if self.odom: - x = self.odom.pose.pose.position.x - y = self.odom.pose.pose.position.y - # orientation as quaternion - qz = self.odom.pose.pose.orientation.z - qw = self.odom.pose.pose.orientation.w + x, y = self.odom.position # meters, odom frame + heading = self.odom.theta_degrees # yaw, counter-clockwise positive + speed = self.odom.linear_velocity # m/s, forward ``` +| Attribute | Type | Description | +| --- | --- | --- | +| `x`, `y` | `float` | Position in meters, odom frame | +| `position` | `tuple[float, float]` | `(x, y)` shorthand | +| `theta` | `float` | Yaw in radians, counter-clockwise positive, wrapped to `(-pi, pi]` | +| `theta_degrees` | `float` | Yaw in degrees (matches `navigate_to_position`'s `theta_degrees`) | +| `linear_velocity` | `float` | Forward speed in m/s (negative when reversing) | +| `angular_velocity` | `float` | Turn rate in rad/s, counter-clockwise positive | +| `stamp` | `float` | Sensor timestamp in seconds | + + +Skills written against releases up to 0.6.x read odometry as a raw-message +dict (`self.odom["theta_degrees"]`, `self.odom["pose"]["pose"]["position"]`). +Dict-style access still works but is deprecated — use the attributes above. + + ## Map The occupancy grid map: @@ -164,17 +180,14 @@ class MonitorPosition(Skill): if not self.odom: return "Odometry not available", SkillResult.FAILURE - start_x = self.odom.pose.pose.position.x - start_y = self.odom.pose.pose.position.y + start = self.odom.position start_time = time.time() while time.time() - start_time < duration: if self._cancelled: return "Monitoring cancelled", SkillResult.CANCELLED - x = self.odom.pose.pose.position.x - y = self.odom.pose.pose.position.y - distance = math.sqrt((x - start_x)**2 + (y - start_y)**2) + distance = math.dist(self.odom.position, start) self._send_feedback(f"Moved {distance:.2f}m from start") time.sleep(0.5) From 41b1d0d32c4ca97e2f416634cc722f2baf6133c6 Mon Sep 17 00:00:00 2001 From: theo-michel Date: Thu, 9 Jul 2026 20:41:32 -0700 Subject: [PATCH 2/5] docs(skills): document odom.raw escape hatch --- software/skills/code-defined-skills/robot-state.mdx | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/software/skills/code-defined-skills/robot-state.mdx b/software/skills/code-defined-skills/robot-state.mdx index b2f3eec..67c9fcb 100644 --- a/software/skills/code-defined-skills/robot-state.mdx +++ b/software/skills/code-defined-skills/robot-state.mdx @@ -75,6 +75,19 @@ class MySkill(Skill): | `linear_velocity` | `float` | Forward speed in m/s (negative when reversing) | | `angular_velocity` | `float` | Turn rate in rad/s, counter-clockwise positive | | `stamp` | `float` | Sensor timestamp in seconds | +| `raw` | `dict` | The full `nav_msgs/Odometry` as plain data | + +### Need more than the 2D pose? + +`odom.raw` carries the complete odometry message with rosbridge-style keys — +the real quaternion, `z`, covariances, and the full twist — for skills doing +their own filtering or fusion: + +```python +quat = self.odom.raw["pose"]["pose"]["orientation"] # {"x", "y", "z", "w"} +pose_cov = self.odom.raw["pose"]["covariance"] # 36 floats, row-major +lateral = self.odom.raw["twist"]["twist"]["linear"]["y"] +``` Skills written against releases up to 0.6.x read odometry as a raw-message From d27bd3066d8ee841cbd21726a27f462829cf167e Mon Sep 17 00:00:00 2001 From: theo-michel Date: Thu, 9 Jul 2026 20:47:02 -0700 Subject: [PATCH 3/5] docs(skills): pin odom dict deprecation to intro release 0.3.0 --- software/skills/code-defined-skills/robot-state.mdx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/software/skills/code-defined-skills/robot-state.mdx b/software/skills/code-defined-skills/robot-state.mdx index 67c9fcb..6a953f8 100644 --- a/software/skills/code-defined-skills/robot-state.mdx +++ b/software/skills/code-defined-skills/robot-state.mdx @@ -90,7 +90,7 @@ lateral = self.odom.raw["twist"]["twist"]["linear"]["y"] ``` -Skills written against releases up to 0.6.x read odometry as a raw-message +Skills written for **0.3.0 through 0.6.x** read odometry as a raw-message dict (`self.odom["theta_degrees"]`, `self.odom["pose"]["pose"]["position"]`). Dict-style access still works but is deprecated — use the attributes above. From 66320e9e0c7e18a459a4ef4d7752fe62f6c2abca Mon Sep 17 00:00:00 2001 From: theo-michel Date: Thu, 9 Jul 2026 20:59:34 -0700 Subject: [PATCH 4/5] docs(skills): odom dict access is a permanent compat layer, no removal --- software/skills/code-defined-skills/robot-state.mdx | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/software/skills/code-defined-skills/robot-state.mdx b/software/skills/code-defined-skills/robot-state.mdx index 6a953f8..dbe7ea5 100644 --- a/software/skills/code-defined-skills/robot-state.mdx +++ b/software/skills/code-defined-skills/robot-state.mdx @@ -92,7 +92,9 @@ lateral = self.odom.raw["twist"]["twist"]["linear"]["y"] Skills written for **0.3.0 through 0.6.x** read odometry as a raw-message dict (`self.odom["theta_degrees"]`, `self.odom["pose"]["pose"]["position"]`). -Dict-style access still works but is deprecated — use the attributes above. +Dict-style access is kept as a permanent compatibility layer — those skills +keep working with no scheduled removal. New skills should use the attributes +above. ## Map From d64a871297337e7f339918dd62619853855b0dee Mon Sep 17 00:00:00 2001 From: theo-michel Date: Thu, 9 Jul 2026 21:23:24 -0700 Subject: [PATCH 5/5] docs(skills): worked odometry examples -- closed-loop control, freshness, raw fusion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Illustrate the Odometry attribute API with runnable patterns: a DriveMeters closed-loop skill, the ยฑ180deg-safe heading accumulation, a stamp-based staleness guard, and a FusePose example using odom.raw for quaternion/z/ covariance. --- .../code-defined-skills/robot-state.mdx | 90 ++++++++++++++++++- 1 file changed, 87 insertions(+), 3 deletions(-) diff --git a/software/skills/code-defined-skills/robot-state.mdx b/software/skills/code-defined-skills/robot-state.mdx index dbe7ea5..7ca190d 100644 --- a/software/skills/code-defined-skills/robot-state.mdx +++ b/software/skills/code-defined-skills/robot-state.mdx @@ -77,6 +77,71 @@ class MySkill(Skill): | `stamp` | `float` | Sensor timestamp in seconds | | `raw` | `dict` | The full `nav_msgs/Odometry` as plain data | +Always check for `None` on first access — the first odometry message is one +publish period away when a skill starts. + +### Closing a loop on odometry + +Declared state refreshes at 50 Hz while your skill runs, so you can read the +attributes in a loop to close a control loop. This drives forward a fixed +distance by watching `position`: + +```python +import math +import time +from innate import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult + +class DriveMeters(Skill): + mobility = Interface(InterfaceType.MOBILITY) + odom = RobotState(RobotStateType.LAST_ODOM) + + @property + def name(self): + return "drive_meters" + + def execute(self, distance: float = 0.5): + if self.odom is None: + return "No odometry", SkillResult.FAILURE + + start = self.odom.position # (x, y) snapshot + while math.dist(self.odom.position, start) < distance: + if self._cancelled: + self.mobility.send_cmd_vel(linear_x=0.0) + return "Cancelled", SkillResult.CANCELLED + # duration acts as a deadman: if this loop dies, the base stops + self.mobility.send_cmd_vel(linear_x=0.15, duration=0.5) + time.sleep(0.1) + + self.mobility.send_cmd_vel(linear_x=0.0) + return f"Drove {distance:.2f}m", SkillResult.SUCCESS +``` + +For heading, `theta_degrees` is already wrapped to `(-180, 180]`. Accumulate +wrapped deltas so a turn across the ±180° seam still counts correctly: + +```python +last = self.odom.theta_degrees +turned = 0.0 +while turned < 90.0: + self.mobility.send_cmd_vel(angular_z=0.5, duration=0.5) + time.sleep(0.05) + now = self.odom.theta_degrees + turned += (now - last + 180.0) % 360.0 - 180.0 # signed shortest-arc delta + last = now +``` + +### Checking freshness + +Use `stamp` (seconds) to skip a reading that has gone stale — for example +after a feed hiccup, when the 50 Hz refresh would otherwise hand you a frozen +value: + +```python +age = time.time() - self.odom.stamp +if age > 0.5: + return f"Odometry stale ({age:.1f}s old)", SkillResult.FAILURE +``` + ### Need more than the 2D pose? `odom.raw` carries the complete odometry message with rosbridge-style keys — @@ -84,9 +149,28 @@ the real quaternion, `z`, covariances, and the full twist — for skills doing their own filtering or fusion: ```python -quat = self.odom.raw["pose"]["pose"]["orientation"] # {"x", "y", "z", "w"} -pose_cov = self.odom.raw["pose"]["covariance"] # 36 floats, row-major -lateral = self.odom.raw["twist"]["twist"]["linear"]["y"] +class FusePose(Skill): + odom = RobotState(RobotStateType.LAST_ODOM) + + @property + def name(self): + return "fuse_pose" + + def execute(self): + if self.odom is None: + return "No odometry", SkillResult.FAILURE + + raw = self.odom.raw # full nav_msgs/Odometry as a dict + quat = raw["pose"]["pose"]["orientation"] # {"x", "y", "z", "w"} — true quaternion + z = raw["pose"]["pose"]["position"]["z"] # height (not on the flat API) + pose_cov = raw["pose"]["covariance"] # 36 floats, row-major 6x6 + lateral = raw["twist"]["twist"]["linear"]["y"] # sideways velocity + + # e.g. reject a fix whose position variance is too high + if pose_cov[0] > 0.25: # var(x) > 0.25 m² + return "Pose too uncertain", SkillResult.FAILURE + + return f"z={z:.3f} lateral={lateral:.3f}", SkillResult.SUCCESS ```