API Reference¶

Types¶

Frozen dataclasses used throughout the framework.

Geometric data types for TyGrit.

class TyGrit.types.geometry.SE2Pose(x: float, y: float, theta: float)¶

2D pose in the plane (position + heading).

x: float¶

y: float¶

theta: float¶

Robot state data types for TyGrit.

class TyGrit.types.robot.JointState(names: tuple[str, ...], positions: tuple[float, ...])¶

Named joint positions.

names: tuple[str, ...]¶

positions: tuple[float, ...]¶

class TyGrit.types.robot.RobotState(base_pose: SE2Pose, planning_joints: tuple[float, ...], head_joints: tuple[float, ...])¶

Full robot state: base + arm + head.

base_pose: SE2Pose¶

planning_joints: tuple[float, ...]¶

head_joints: tuple[float, ...]¶

class TyGrit.types.robot.WholeBodyConfig(arm_joints: ndarray[Any, dtype[float64]], base_pose: SE2Pose)¶

Whole-body configuration for planning: arm + base.

arm_joints: ndarray[Any, dtype[float64]]¶

base_pose: SE2Pose¶

class TyGrit.types.robot.IKSolution(joint_names: tuple[str, ...], positions: ndarray[Any, dtype[float64]])¶

A single IK solution, tagged with the joint names it corresponds to.

joint_names: tuple[str, ...]¶

positions: ndarray[Any, dtype[float64]]¶

Sensor data types for TyGrit.

class TyGrit.types.sensor.SensorSnapshot(rgb: ndarray[Any, dtype[uint8]], depth: ndarray[Any, dtype[float32]], intrinsics: ndarray[Any, dtype[float64]], robot_state: RobotState, segmentation: ndarray[Any, dtype[int32]] | None = None)¶

A single synchronised capture from the robot’s sensors.

rgb: ndarray[Any, dtype[uint8]]¶

depth: ndarray[Any, dtype[float32]]¶

intrinsics: ndarray[Any, dtype[float64]]¶

robot_state: RobotState¶

segmentation: ndarray[Any, dtype[int32]] | None = None¶

Planning-related data types for TyGrit.

class TyGrit.types.planning.SchedulerFeedback(is_path_valid: bool | None = None, is_goal_valid: bool | None = None, trajectory_exhausted: bool = False)¶

Information the scheduler passes up to the subgoal generator.

Encapsulates what the low-level execution loop knows so the high-level decision-maker can react without reaching into the planner directly.

is_path_valid: bool | None = None¶

is_goal_valid: bool | None = None¶

trajectory_exhausted: bool = False¶

class TyGrit.types.planning.Trajectory(arm_path: tuple[ndarray[Any, dtype[float64]], ...], base_configs: tuple[SE2Pose, ...])¶

A planned trajectory: sequences of arm configs and base poses.

arm_path: tuple[ndarray[Any, dtype[float64]], ...]¶

base_configs: tuple[SE2Pose, ...]¶

class TyGrit.types.planning.PlanningMode(value)¶

Which planning strategy the scheduler should use for a subgoal.

ARM = 'arm'¶

WHOLE_BODY = 'whole_body'¶

INTERPOLATION = 'interpolation'¶

class TyGrit.types.planning.Subgoal(mode: PlanningMode, arm_joints: ndarray[Any, dtype[float64]], base_pose: SE2Pose | None = None)¶

A subgoal returned by the subgoal generator.

Carries the goal configuration and the planning mode so the scheduler knows which planner method to call.

mode: PlanningMode¶

arm_joints: ndarray[Any, dtype[float64]]¶

base_pose: SE2Pose | None = None¶

Grasp-related data types.

class TyGrit.types.grasp.GraspPose(transform: ndarray[Any, dtype[float64]], score: float)¶

A candidate grasp with a quality score.

transform: ndarray[Any, dtype[float64]]¶

score: float¶

Result and outcome types for TyGrit subsystems.

Every subsystem that can succeed or fail returns a typed result dataclass. Grouping them here makes it easy to find and extend the vocabulary of outcomes without hunting through logic modules.

class TyGrit.types.results.PlanResult(success: bool, trajectory: ~TyGrit.types.planning.Trajectory | None = None, failure: ~TyGrit.types.failures.PlannerFailure | None = None, stats: dict[str, float] = <factory>)¶

Result of a motion-planning query.

success: bool¶

trajectory: Trajectory | None = None¶

failure: PlannerFailure | None = None¶

stats: dict[str, float]¶

class TyGrit.types.results.StageResult(success: bool, failure: Enum | None = None, message: str = '')¶

Outcome of a pipeline stage (grasp / prepose / observe / place).

The failure field accepts any subsystem failure enum — the stage doesn’t flatten the type, it just forwards whatever the subsystem reported.

success: bool¶

failure: Enum | None = None¶

message: str = ''¶

class TyGrit.types.results.SchedulerOutcome(value)¶

Possible outcomes of a scheduler run.

SUCCESS = 'success'¶

MAX_ITERATIONS = 'max_iterations'¶

PLAN_FAILURE = 'plan_failure'¶

class TyGrit.types.results.SchedulerResult(outcome: ~TyGrit.types.results.SchedulerOutcome, iterations: int = 0, total_steps: int = 0, stats: dict[str, float] = <factory>)¶

Result returned by Scheduler.run().

outcome: SchedulerOutcome¶

iterations: int = 0¶

total_steps: int = 0¶

stats: dict[str, float]¶

Per-subsystem failure enums.

Each subsystem defines its own failure type. These are leaf labels — they describe what failed, not why the overall system failed.

Causal analysis (building the failure tree, identifying root cause) is done separately by the analysis system, not here.

Add new enums as subsystems are built. Keep variants minimal — only failures we’ve actually observed or designed for.

class TyGrit.types.failures.PlannerFailure(value)¶

Motion planner failures.

COLLISION_AT_START = 'collision_at_start'¶

COLLISION_AT_GOAL = 'collision_at_goal'¶

NO_PATH_FOUND = 'no_path_found'¶

TIMEOUT = 'timeout'¶

class TyGrit.types.failures.IKFailure(value)¶

Inverse kinematics failures.

NO_SOLUTION = 'no_solution'¶

JOINT_LIMITS = 'joint_limits'¶

class TyGrit.types.failures.GraspFailure(value)¶

Grasp prediction failures.

NO_GRASPS_DETECTED = 'no_grasps_detected'¶

ALL_UNREACHABLE = 'all_unreachable'¶

class TyGrit.types.failures.PerceptionFailure(value)¶

Perception subsystem failures.

SEGMENTATION_FAILED = 'segmentation_failed'¶

DEPTH_INVALID = 'depth_invalid'¶

class TyGrit.types.failures.ExecutionFailure(value)¶

Trajectory execution failures.

COLLISION_DETECTED = 'collision_detected'¶

TIMEOUT = 'timeout'¶

JOINT_ERROR = 'joint_error'¶

Utilities¶

Pure functions for math, transforms, and point cloud processing.

Low-level math helpers: angle wrapping, quaternion / matrix conversions.

All quaternions use [x, y, z, w] convention (SciPy / ROS).

TyGrit.utils.math.angle_wrap(angle: float) → float¶: Wrap angle (radians) into (-pi, pi].

TyGrit.utils.math.quaternion_to_matrix(quaternion: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float64]]¶

Convert quaternion [x, y, z, w] to a 4x4 homogeneous matrix.

Ported from grasp_anywhere.robot.utils.transform_utils.quaternion_matrix.

TyGrit.utils.math.matrix_to_quaternion(matrix: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float64]]¶

Extract [x, y, z, w] quaternion from the rotation part of a 4x4 matrix.

Ported from grasp_anywhere.robot.utils.transform_utils.quaternion_from_matrix.

TyGrit.utils.math.translation_from_matrix(matrix: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float64]]¶: Extract the translation vector from a 4x4 homogeneous matrix.

Coordinate-frame transform utilities.

Ported from grasp_anywhere.robot.utils.transform_utils. All quaternions use [x, y, z, w] convention.

TyGrit.utils.transforms.create_pose_matrix(position: ndarray[Any, dtype[float64]], quaternion: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float64]]¶: Build a 4x4 homogeneous matrix from position and quaternion [x,y,z,w].

TyGrit.utils.transforms.create_transform_matrix(translation: ndarray[Any, dtype[float64]], rotation_matrix: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float64]]¶: Build a 4x4 homogeneous matrix from a 3-vector translation and a 3x3 rotation.

TyGrit.utils.transforms.pose_to_world(base_pos: ndarray[Any, dtype[float64]], base_yaw: float, ee_pos: ndarray[Any, dtype[float64]], ee_quat: ndarray[Any, dtype[float64]]) → tuple[ndarray[Any, dtype[float64]], ndarray[Any, dtype[float64]]]¶

Transform a pose from the robot base frame to the world frame.

Parameters:

base_pos – [x, y, z] of the base in world frame.
base_yaw – Heading of the base (radians).
ee_pos – [x, y, z] in base frame.
ee_quat – [x, y, z, w] in base frame.

Returns:

(world_pos, world_quat) – both as numpy arrays.

TyGrit.utils.transforms.pose_to_base(world_pos: ndarray[Any, dtype[float64]], world_quat: ndarray[Any, dtype[float64]], base_pos: ndarray[Any, dtype[float64]], base_yaw: float) → tuple[ndarray[Any, dtype[float64]], ndarray[Any, dtype[float64]]]¶

Transform a pose from the world frame to the robot base frame.

Parameters:

world_pos – [x, y, z] in world frame.
world_quat – [x, y, z, w] in world frame.
base_pos – [x, y, z] of the base in world frame.
base_yaw – Heading of the base (radians).

Returns:

(ee_pos, ee_quat) – in the base frame.

TyGrit.utils.transforms.se2_to_matrix(x: float, y: float, theta: float) → ndarray[Any, dtype[float64]]¶: Convert an SE(2) pose to a 4x4 homogeneous matrix (rotation about Z).

Point-cloud utilities: downsample, merge, crop, filter.

Ported from grasp_anywhere.observation.scene.Scene static methods. All functions are pure – they take arrays in and return arrays out.

TyGrit.utils.pointcloud.voxel_downsample(points: ndarray[Any, dtype[float32]], voxel_size: float) → ndarray[Any, dtype[float32]]¶

Down-sample points (N, 3) with a voxel grid of size voxel_size.

Keeps one representative point per voxel (the first encountered). Pure-numpy implementation – no Open3D dependency.

TyGrit.utils.pointcloud.merge_dedup(base: ndarray[Any, dtype[float32]], add: ndarray[Any, dtype[float32]], radius: float) → ndarray[Any, dtype[float32]]¶

Merge add into base, de-duplicating via a voxel grid of size radius.

Points in base always take priority (are kept). Ported from Scene._merge_dedup.

TyGrit.utils.pointcloud.crop_sphere(points: ndarray[Any, dtype[float32]], center: ndarray[Any, dtype[float32]], radius: float) → ndarray[Any, dtype[float32]]¶

Keep only points within a 2-D cylinder (XY distance ≤ radius) from center.

Ported from Scene._crop_sphere.

TyGrit.utils.pointcloud.filter_ground(points: ndarray[Any, dtype[float32]], z_threshold: float) → ndarray[Any, dtype[float32]]¶: Remove points at or below z_threshold (the ground plane).

TyGrit.utils.pointcloud.points_in_frustum_mask(points: ndarray[Any, dtype[float32]], intrinsics: ndarray[Any, dtype[float64]], extrinsics: ndarray[Any, dtype[float64]], z_range: tuple[float, float], image_width: int = 640, image_height: int = 480) → ndarray[Any, dtype[bool_]]¶

Return a boolean mask for points that fall inside the camera frustum.

Parameters:

points – (N, 3) world-frame points.
intrinsics – (3, 3) camera intrinsic matrix K.
extrinsics – (4, 4) world-to-camera T_wc.
z_range – (z_min, z_max) depth bounds in camera frame.
image_width – Sensor width in pixels.
image_height – Sensor height in pixels.

Ported from Scene._points_in_frustum_mask.

Depth-image utilities: back-projection to point clouds, image projection.

Ported from grasp_anywhere.utils.perception_utils. All functions are pure – no robot or ROS references.

TyGrit.utils.depth.depth_to_pointcloud(depth: ndarray[Any, dtype[float32]], intrinsics: ndarray[Any, dtype[float64]], stride: int = 1) → ndarray[Any, dtype[float32]]¶

Back-project a depth image to a camera-frame point cloud.

Parameters:

depth – (H, W) depth map in metres.
intrinsics – (3, 3) camera intrinsic matrix K.
stride – Decimation factor (1 = no decimation).

Returns:

(N, 3) point cloud in the camera frame. Points with depth ≤ 0 are excluded.

Ported from grasp_anywhere.utils.perception_utils.depth2pc.

TyGrit.utils.depth.depth_to_world_pointcloud(depth: ndarray[Any, dtype[float32]], intrinsics: ndarray[Any, dtype[float64]], extrinsics: ndarray[Any, dtype[float64]], z_range: tuple[float, float] = (0.2, 3.0), stride: int = 4) → ndarray[Any, dtype[float32]]¶

Back-project depth and transform to world frame in one call.

Parameters:

depth – (H, W) depth map in metres (NaN-safe).
intrinsics – (3, 3) camera intrinsic matrix K.
extrinsics – (4, 4) camera-to-world matrix T_wc.
z_range – (z_min, z_max) depth bounds in camera frame.
stride – Decimation factor.

Returns:

(N, 3) world-frame point cloud (float32).

TyGrit.utils.depth.pointcloud_from_mask(depth: ndarray[Any, dtype[float32]], mask: ndarray[Any, dtype[bool_]], intrinsics: ndarray[Any, dtype[float64]]) → ndarray[Any, dtype[float32]]¶

Extract a camera-frame point cloud for pixels where mask is True.

Ported from grasp_anywhere.utils.perception_utils.get_pcd_from_mask.

Returns:: (N, 3) point cloud, or an empty (0, 3) array when no valid points exist.

TyGrit.utils.depth.project_points_to_image(points: ndarray[Any, dtype[float32]], intrinsics: ndarray[Any, dtype[float64]], extrinsics: ndarray[Any, dtype[float64]]) → tuple[ndarray[Any, dtype[float64]], ndarray[Any, dtype[float64]], ndarray[Any, dtype[float32]]]¶

Project world-frame 3-D points onto the image plane.

Parameters:

points – (N, 3) in world frame.
intrinsics – (3, 3) camera K.
extrinsics – (4, 4) world-to-camera T_wc.

Returns:

(u, v, depth) – pixel coordinates and depth in camera frame.

Protocols¶

Protocol for motion planning.

class TyGrit.planning.motion_planner.MotionPlanner(*args, **kwargs)¶

Plans collision-free trajectories.

update_environment(points: ndarray[Any, dtype[float32]], base_pose: SE2Pose) → None¶

Sync the planner’s collision world.

Called once per scheduler iteration, after the scene is updated.

Parameters:

points – (N, 3) filtered world-frame point cloud.
base_pose – Current base pose — needed so arm-only planning knows where the base sits.

plan_arm(start: ndarray[Any, dtype[float64]], goal: ndarray[Any, dtype[float64]]) → PlanResult¶

plan_whole_body(start: WholeBodyConfig, goal: WholeBodyConfig) → PlanResult¶

plan_interpolation(start: ndarray[Any, dtype[float64]], goal: ndarray[Any, dtype[float64]], base_pose: SE2Pose) → PlanResult¶

Linear joint-space interpolation (no collision checking).

Used for short, contact-rich motions (grasp, lift) where collision-aware planners would reject the goal.

Parameters:

start – (8,) current arm joint config.
goal – (8,) target arm joint config.
base_pose – Current base pose (held constant throughout).

validate_config(config: WholeBodyConfig) → bool¶

Protocol for robot environment implementations.

class TyGrit.envs.base.RobotBase(*args, **kwargs)¶

Protocol every robot environment (sim / real) must satisfy.

Robot-specific capabilities (torso, specific joint control, MPC, gaze optimisation) belong in concrete subclasses (e.g. FetchRobot), not here.

property camera_ids: list[str]¶: Available camera identifiers (e.g. ["head", "wrist"]).

get_sensor_snapshot(camera_id: str) → SensorSnapshot¶: Capture RGB-D + robot state from the specified camera.

get_robot_state() → RobotState¶

look_at(target: ndarray[Any, dtype[float64]], camera_id: str) → None¶

Point camera_id at a 3-D world-frame target [x, y, z].

The implementation resolves this to whatever mechanism the robot uses (pan-tilt head, arm IK for wrist camera, etc.). Not all cameras are steerable — implementations should raise NotImplementedError for fixed cameras.

step(action: ndarray[Any, dtype[float32]]) → SensorSnapshot¶

Apply action for one simulation/control step and return new obs.

The action vector is robot-specific (e.g. joint velocities). No background thread — the caller controls the loop rate.

get_observation() → SensorSnapshot¶: Return the latest observation without stepping.

execute_trajectory(trajectory: Trajectory) → bool¶

start_trajectory(trajectory: Trajectory) → None¶

stop_motion() → None¶

is_motion_done() → bool¶

control_gripper(position: float) → None¶

reset() → SensorSnapshot¶: Reset the environment and return a fresh observation.

close() → None¶