Robot State

from innate import MainImage, Odometry, Skill


class MySkill(Skill):
    """..."""

    image: MainImage
    odom: Odometry

    def execute(self):
        x, y = self.odom.position
        self.logger.info(f"at ({x:.2f}, {y:.2f}), yaw {self.odom.theta_degrees:.0f}°")

class MySkill(Skill):
    """..."""

    odom: Odometry
    pose: Pose

    def execute(self):
        self.odom.x, self.odom.y          # metres, odom frame
        self.odom.theta                   # radians, CCW, wrapped to (-pi, pi]
        self.odom.theta_degrees           # the same in degrees
        self.odom.position                # (x, y)
        self.odom.linear_velocity         # m/s, negative when reversing
        self.odom.angular_velocity        # rad/s
        self.odom.stamp                   # sensor time, seconds

        self.pose.x, self.pose.y, self.pose.theta   # the same shape, map frame

import io

from PIL import Image as PILImage

from innate import MainImage, Skill, WristImage


class MySkill(Skill):
    """..."""

    image: MainImage
    wrist_image: WristImage

    def execute(self):
        response = vision_api.analyze(self.image)          # already base64 text
        img = PILImage.open(io.BytesIO(self.image.jpeg))   # or raw bytes

class MySkill(Skill):
    """..."""

    lidar: Lidar

    def execute(self):
        ahead = self.lidar.min_range(-20, 20)     # closest return in a ±20° wedge
        if ahead is not None and ahead < 0.4:
            self.fail("Something is right in front of me")

        self.lidar.ranges            # metres, one per beam
        self.lidar.angle_min         # angle of beam 0, radians
        self.lidar.angle_increment   # radians between beams

class MySkill(Skill):
    """..."""

    arm: Arm
    joint_states: JointStates
    head_position: HeadState

    def execute(self):
        self.arm.x, self.arm.y, self.arm.z    # end-effector, metres
        self.arm.rpy                          # (roll, pitch, yaw), radians
        self.arm.gripper                      # claw joint, radians

        self.joint_states.of("joint2")        # (position, velocity, effort)

        self.head_position.pitch_degrees      # current head tilt

class MySkill(Skill):
    """..."""

    map: Map

    def execute(self):
        self.map.resolution                       # metres per cell
        self.map.width, self.map.height           # cells
        self.map.origin_x, self.map.origin_y      # world position of cell (0, 0)

        grid = self.map.grid                      # (height, width) int8 numpy array
        # -1 unknown, 0 free, 100 occupied

import math

from innate import MainImage, Mobility, Skill


class CaptureImages(Skill):
    """Capture images from several directions. Use to survey a room before
    deciding where to go."""

    mobility: Mobility
    image: MainImage

    def execute(self, num_directions: int = 4):
        images = []
        step = (2 * math.pi) / num_directions

        for i in range(num_directions):
            images.append(str(self.image))
            self.feedback(f"Captured {i + 1}/{num_directions}")
            if i < num_directions - 1:
                self.mobility.rotate(step)

        return f"Captured {len(images)} images"

import math
import time

from innate import Odometry, Skill


class MonitorPosition(Skill):
    """Watch how far the robot moves over a duration. Use to check whether
    something is pushing the robot around."""

    odom: Odometry

    def execute(self, duration: float = 5.0):
        start = self.odom.position
        deadline = time.monotonic() + duration

        while time.monotonic() < deadline:
            moved = math.dist(self.odom.position, start)
            self.feedback(f"Moved {moved:.2f}m from start")
            self.sleep(0.5)

        return f"Moved {math.dist(self.odom.position, start):.2f}m in {duration:.0f}s"