# === planner_interface ===
# Represents a 3D pose with position and orientation
class Pose:
    p: NDArray[np.float64]  # Position [x, y, z]
    q: NDArray[np.float64]  # Quaternion [w, x, y, z]
    # Pose from position p [x, y, z] and quaternion q [w, x, y, z] (q may be unnormalized)
    def __init__(self, p: Optional[NDArray[np.float64]]=None, q: Optional[NDArray[np.float64]]=None) -> None: ...
    # Pose from a 4x4 transformation matrix
    def __init__(self, matrix: NDArray[np.float64]) -> None: ...
    # Compose two poses
    def __mul__(self, other: Pose) -> Pose: ...
    # Transform a 3D point by this pose
    def __mul__(self, point: NDArray[np.float64]) -> NDArray[np.float64]: ...
    # Inverse pose
    def inv(self) -> Pose: ...
    # Distance between two poses
    def distance(self, other: Pose) -> float: ...
    # 4x4 transformation matrix of this pose
    def to_transformation_matrix(self) -> NDArray[np.float64]: ...

# Goal constraint kinds.
class GoalType:
    JOINTS: int
    POSE: int
    POSITION: int
    MULTI_GOAL_JOINTS: int
    MULTI_GOAL_POSE: int
    MULTI_GOAL_POSITION: int

# Goal constraint for motion planning
class GoalConstraint:
    type: int
    joints: List[NDArray[np.float64]]
    poses: List[Pose]
    positions: List[NDArray[np.float64]]
    # Empty goal constraint
    def __init__(self) -> None: ...
    # Goal constraint of the given GoalType
    def __init__(self, type: int) -> None: ...
    # For JOINTS / MULTI_GOAL_JOINTS
    def __init__(self, type: int, joints: List[NDArray[np.float64]]) -> None: ...
    # For POSITION / MULTI_GOAL_POSITION, each [x, y, z]
    def __init__(self, type: int, positions: List[NDArray[np.float64]]) -> None: ...
    # For POSE / MULTI_GOAL_POSE
    def __init__(self, type: int, poses: List[Pose]) -> None: ...

# Represents a robot trajectory
class Trajectory:
    positions: List[List[float]]
    velocities: List[List[float]]
    accelerations: List[List[float]]
    times: List[float]
    def __init__(self) -> None: ...

# Occupancy-grid bounds and resolution.
class GridConfig:
    origin_x: float  # default -1.0
    origin_y: float  # default -1.0
    origin_z: float  # default  0.0
    size_x: float  # default  2.0
    size_y: float  # default  2.0
    size_z: float  # default  2.0
    resolution: float  # default  0.02
    max_distance: float  # default 0.2
    ref_counted: bool  # default True -- consumed by the distance-field constructor
    def __init__(self) -> None: ...

# The occupancy grid and distance field of the planning world.
class OccupancyGrid:
    # World coordinates of the grid's minimum corner
    @property
    def origin(self) -> Tuple[float, float, float]: ...
    # Extent of the grid in meters, per axis
    @property
    def size(self) -> Tuple[float, float, float]: ...
    # Axis-aligned world bounds, as (min_xyz, max_xyz)
    @property
    def bounds(self) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]: ...
    # Cell side length in meters
    @property
    def resolution(self) -> float: ...
    # Number of cells along each axis
    @property
    def num_cells(self) -> Tuple[int, int, int]: ...
    # Number of cells currently marked occupied
    @property
    def occupied_voxel_count(self) -> int: ...
    reference_frame: str  # Name of the frame the grid is expressed in
    # All occupied voxel centers in the grid, as an (N, 3) world-frame array
    def get_occupied_voxels(self) -> NDArray[np.float64]: ...
    # Occupied voxel centers within a sphere of the given radius, as an (N, 3) array
    def get_occupied_voxels(self, center: NDArray[np.float64], radius: float) -> NDArray[np.float64]: ...
    # Occupied voxel centers inside the box at pose with extents dims, as an (N, 3) array
    def get_occupied_voxels(self, pose: Pose, dims: List[float]) -> NDArray[np.float64]: ...
    # Distance to the nearest occupied cell, clamped to the grid's max_distance
    def get_distance_from_point(self, x: float, y: float, z: float) -> float: ...
    # Squared distance to the nearest occupied cell, in meters^2
    def get_squared_distance_from_point(self, x: float, y: float, z: float) -> float: ...
    # Distance from a world point to the nearest grid face; 0.0 if outside
    def get_distance_to_border(self, x: float, y: float, z: float) -> float: ...
    # Distance from a cell to the nearest occupied cell, in meters
    def get_cell_distance(self, i: int, j: int, k: int) -> float: ...
    # Distance from a cell to the nearest grid face; 0.0 if outside
    def get_cell_distance_to_border(self, i: int, j: int, k: int) -> float: ...
    # World coordinates of a cell's center
    def grid_to_world(self, i: int, j: int, k: int) -> Tuple[float, float, float]: ...
    # Indices of the cell containing a world point; not clamped to bounds
    def world_to_grid(self, x: float, y: float, z: float) -> Tuple[int, int, int]: ...
    # Whether a world point falls inside the grid
    def is_in_bounds(self, x: float, y: float, z: float) -> bool: ...
    # Whether cell indices fall inside the grid
    def is_cell_in_bounds(self, i: int, j: int, k: int) -> bool: ...

# Main interface for motion planning with IMS
class PlannerInterface:
    # Constructs a PlannerInterface with optional grid configuration
    def __init__(self, grid_config: Optional[GridConfig]=None) -> None: ...
    # Adds an articulation to the planning world
    def add_articulation(self, name: str, end_effector: Optional[str]=None, urdf_path: str='', srdf_path: str='', link_names: List[str]=[], joint_names: List[str]=[], gravity: NDArray[np.float64]=np.array([0, 0, 0]), planned: bool=True, gripper_joint_names: List[str]=[], end_effectors: Optional[List[str]]=None) -> str: ...
    # Add a robot from the registry or by file path.
    def add_robot(self, robot: str, name: Optional[str]=None, srdf_path: Optional[str]=None, end_effector: Optional[str]=None, planned: bool=True, gravity: Optional[NDArray[np.float64]]=None, link_names: Optional[List[str]]=None, joint_names: Optional[List[str]]=None, gripper_joint_names: Optional[List[str]]=None) -> str: ...
    # Start and attach a visualizer to the planner.
    def start_visualizer(self, type: str='viser', **kwargs) -> Any: ...
    # Write the planning world to `path` as a JSON manifest (plus a .npz sidecar when the scene holds geometry with no file behind it).
    def save_scene(self, path: Union[str, Path]) -> Path: ...
    # Rebuild this planner's world from a manifest written by save_scene().
    def load_scene(self, path: Union[str, Path], clear: bool=True) -> None: ...
    # Write the most recent plan() / plan_multi() episode to `path`.
    def save_plan(self, path: Union[str, Path]) -> Path: ...
    # Get an attached visualizer.
    def get_visualizer(self, index: int=0) -> Any: ...
    # Attach a VisualizerListener to the planner (low-level)
    def attach_visualizer(self, listener: Any) -> None: ...
    # Detach a VisualizerListener from the planner (low-level)
    def detach_visualizer(self, listener: Any) -> None: ...
    # Removes an articulation from the planning world
    def remove_articulation(self, name: str) -> None: ...
    # Sets the base pose of the articulation
    def set_base_pose(self, name: str, pose: Pose) -> None: ...
    # Read the simulation world objects from sim of type sim_type
    def read_sim(self, sim: Any, sim_type: str, articulations: Optional[List[Any]]=None) -> None: ...
    # Adds a mesh to the planning world from a file
    def add_mesh(self, name: str, mesh_path: str='', scale: Union[NDArray[np.float64], List[float]]=..., pose: Pose=..., convex: bool=False, *, vertices: None=None, triangles: None=None, color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Adds a mesh to the planning world from vertices and triangles
    def add_mesh(self, name: str, mesh_path: str='', scale: Union[NDArray[np.float64], List[float]]=..., pose: Pose=..., convex: bool=False, *, vertices: NDArray[np.float64], triangles: NDArray[np.int32], color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Adds a mesh to the planning world
    def add_mesh(self, name: str, mesh_path: str='', scale: Union[NDArray[np.float64], List[float]]=..., pose: Pose=..., convex: bool=False, vertices: Optional[NDArray[np.float64]]=None, triangles: Optional[NDArray[np.int32]]=None, color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Sets how an object is drawn by attached visualizers (display only; saved with the scene)
    def set_object_color(self, name: str, color: Union[Tuple[float, float, float], List[float], str]) -> None: ...
    # Adds a box to the planning world
    def add_box(self, name: str, size: Union[NDArray[np.float64], List[float]], pose: Pose=Pose(), color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Adds a sphere to the planning world
    def add_sphere(self, name: str, radius: float, pose: Pose=Pose(), color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Adds a cylinder to the planning world
    def add_cylinder(self, name: str, radius: float, length: float, pose: Pose=Pose(), color: Optional[Union[Tuple[float, float, float], List[float], str]]=None) -> None: ...
    # Adds a point cloud to the planning world
    def add_point_cloud(self, name: str, points: NDArray[np.float64], colors: Optional[NDArray[np.float64]]=None, pose: Pose=Pose()) -> None: ...
    # Removes an object from the planning world
    def remove_object(self, name: str) -> None: ...
    # Move an existing object to a new pose
    def update_object_pose(self, name: str, pose: Pose) -> None: ...
    # Replace the occupancy-grid configuration of the planning world
    def set_grid(self, grid_config: GridConfig) -> None: ...
    # Get names of all articulations in the planning world
    def get_articulation_names(self) -> List[str]: ...
    # Get names of all objects in the planning world
    def get_object_names(self) -> List[str]: ...
    # Get names of all active (user-defined) joints for an articulation.
    def get_active_joint_names(self, articulation_name: str) -> List[str]: ...
    # Get the articulation's current qpos, in move-group order (same order set_qpos/plan expect).
    def get_qpos(self, articulation_name: str) -> NDArray[np.float64]: ...
    # Get names of the move-group joints for an articulation.
    def get_move_group_joint_names(self, articulation_name: str) -> List[str]: ...
    # Get position limits for an articulation's move-group joints, in the same order as set_qpos/plan (and get_jacobian's move-group columns). Only supports single-DOF joints (revolute/prismatic/continuous) -- raises otherwise.
    def get_move_group_joint_limits(self, articulation_name: str) -> NDArray[np.float64]: ...
    # Get names of the gripper joints for an articulation.
    def get_gripper_joint_names(self, articulation_name: str) -> List[str]: ...
    # Set joint angles for an articulation's gripper joints
    def set_gripper_qpos(self, robot_name: str, gripper_qpos: Union[NDArray[np.float64], List[float]]) -> None: ...
    # Get the move-group velocity-space DOF for an articulation.
    def get_move_group_qpos_dim(self, articulation_name: str) -> int: ...
    # Get link index from link name for an articulation
    def get_link_index(self, articulation_name: str, link_name: str) -> int: ...
    # Get link names for an articulation
    def get_link_names(self, articulation_name: str) -> List[str]: ...
    # Get the Jacobian of a link at the articulation's current qpos.
    def get_jacobian(self, articulation_name: str, link_name: str, local: bool=False, move_group_only: bool=True) -> NDArray[np.float64]: ...
    # Get the Jacobian of a link at an arbitrary qpos, without touching the articulation's current internal state.
    def compute_jacobian(self, articulation_name: str, qpos: Union[NDArray[np.float64], List[float]], link_name: str, local: bool=False, move_group_only: bool=True) -> NDArray[np.float64]: ...
    # Map a desired end-effector twist to move-group joint velocities via damped least squares (robust near/at singularities).
    def compute_joint_velocities(self, articulation_name: str, link_name: str, twist: Union[NDArray[np.float64], List[float]], damping: float=0.0, local: bool=False) -> NDArray[np.float64]: ...
    # Yoshikawa manipulability index of a link's move-group Jacobian: sqrt(det(J J^T)). 0 at a kinematic singularity; higher means more dexterous. Use to detect/avoid singularities during control or planning.
    def get_manipulability(self, articulation_name: str, link_name: str) -> float: ...
    # Check if articulation exists
    def has_articulation(self, name: str) -> bool: ...
    # Check if object exists
    def has_object(self, name: str) -> bool: ...
    # Check if articulation is being planned
    def is_articulation_planned(self, name: str) -> bool: ...
    # Set whether articulation is being planned
    def set_articulation_planned(self, name: str, planned: bool) -> None: ...
    # Check if object is attached to a robot
    def is_object_attached(self, name: str) -> bool: ...
    # Attach object to robot link
    def attach_object(self, object_name: str, robot_name: str, link_id: Union[int, str], touch_links: Optional[List[str]]=None, is_link_name: bool=False, *, pose: Optional[Pose]=None) -> None: ...
    # Detach object from robot
    def detach_object(self, name: str, also_remove: bool=False) -> bool: ...
    # Detach all attached objects
    def detach_all_objects(self, also_remove: bool=False) -> bool: ...
    # Check if current state is in collision
    def is_state_colliding(self, articulation_name: str='') -> bool: ...
    # Check if robot is colliding with objects
    def is_robot_colliding_with_objects(self, art_name: str) -> bool: ...
    # Get minimum distance to self-collision
    def distance_to_self_collision(self) -> float: ...
    # Get minimum distance to environment collision
    def distance_to_robot_collision(self) -> float: ...
    # Get minimum distance to any collision
    def distance_to_collision(self) -> float: ...
    # Allow (or disallow) collisions between two objects, robots or robot links.
    def set_allowed_collision(self, name1: str, name2: str, allowed: bool) -> None: ...
    # Set joint positions for articulation
    def set_qpos(self, robot_name: str, qpos: NDArray[np.float64]) -> None: ...
    # Set joint positions for all planned articulations
    def set_qpos_all(self, state: NDArray[np.float64]) -> None: ...
    # Update poses of all attached bodies based on current robot state
    def update_attached_bodies_pose(self) -> None: ...
    # Compute forward kinematics for an articulation
    def compute_fk(self, articulation_name: str, qpos: NDArray[np.float64]) -> Pose: ...
    # Compute inverse kinematics using CLIK with joint limits
    def compute_ik(self, articulation_name: str, ee_pose: List[float], init_state_val: List[float]) -> Tuple[bool, List[float]]: ...
    # Compute inverse kinematics using CLIK with joint limits (Pose overload)
    def compute_ik(self, articulation_name: str, ee_pose: Pose, init_state_val: List[float]) -> Tuple[bool, List[float]]: ...
    # Collision-aware IK via rejection sampling with random restarts. Returns (q, status): status "found" (q is a collision-free configuration and the robot is left at it), "unreachable" (IK never succeeded within timeout; q is None) or "blocked" (IK succeeded but every solution collides; q is None).
    def collision_aware_ik(self, articulation_name: str, ee_pose: Union[Pose, List[float]], q_seed: List[float], timeout: float=0.05, perturbation_steps: int=5, perturbation_sigma_max: float=0.3, best_of: bool=False) -> Tuple[Optional[List[float]], str]: ...
    # Get link pose for an articulation
    def get_link_pose(self, articulation_name: str, link_name: str) -> Pose: ...
    # Return the validity backends compiled into this srmp build.
    def get_available_validity_backends(self) -> List[str]: ...
    # Return the backend attached to the current single-agent planner.
    def get_active_validity_backend(self) -> str: ...
    # Statistics of the most recent plan, measured by the planner itself.
    def get_planning_stats(self) -> Dict[str, Any]: ...
    # Root graphs MGS added during the most recent plan, in injection order.
    def get_mgs_roots(self) -> List[Tuple[int, NDArray[np.float64]]]: ...
    # VAMP's verdict on a configuration: True means collision-free (the opposite polarity of is_state_colliding)
    def is_vamp_state_valid(self, articulation_name: str, q: List[float]) -> bool: ...
    # Robot names with a VAMP Robot_Module in this build (sorted).
    def get_vamp_supported_robots(self) -> List[str]: ...
    # Whether the VAMP backend can attach to this articulation.
    def is_robot_vamp_supported(self, articulation_name: str) -> bool: ...
    # Makes a planner with given articulation names and configuration
    def make_planner(self, articulation_names: List[str], planner_context: Dict[str, str]) -> None: ...
    # Plan a motion for a single agent
    def plan(self, start: List[float], goal_constraint: GoalConstraint) -> Trajectory: ...
    # Plan motions for multiple agents simultaneously
    def plan_multi(self, start_states: Dict[str, List[float]], goal_constraints: Dict[str, GoalConstraint]) -> Dict[str, Trajectory]: ...
    # Cartesian screw-motion plan for a link via Jacobian resolved-rate stepping (no IK solve)
    def plan_screw(self, articulation_name: str, link_name: str, start_qpos: Optional[NDArray[np.float64]]=None, end_pose: Optional[Pose]=None, axis_point: Optional[NDArray[np.float64]]=None, axis_direction: Optional[NDArray[np.float64]]=None, pitch: float=0.0, angle: Optional[float]=None, qpos_step: float=0.1, max_steps: int=10000, collision_aware: bool=True) -> Trajectory: ...
    # Get the occupancy grid backing the distance field and BFS heuristics
    def get_grid(self) -> OccupancyGrid: ...
    # Reset the planner interface
    def reset(self, reset_robots: bool=True) -> None: ...
    # Print list of available planners
    def print_available_planners(self) -> None: ...

# === Planner selection and planner_context ===
# make_planner(articulation_names, planner_context): all planner_context values are strings;
# keys and planner names are case-sensitive. print_available_planners() lists what this build has.
#   planner_id: single-agent "Astar" / "wAstar" / "ARAstar" / "MHAstar" / "wPASE" / "MGS" / "vamp_rrtc"
#               (vamp_rrtc: VAMP RRT-Connect, joint-space goals, needs the VAMP backend);
#               multi-agent (len(articulation_names) > 1) "ECBS" / "xECBS"
#   MGS only: heuristic (root-to-root, default "joint_steps"; not "bfs"),
#             anchor_heuristic (default "bfs"), g_num (graphs, default "10"), weight (default "100"),
#             anchor_search ("true"/"false", default "true"), root_ik_threads (default: half the hardware threads),
#             anchor_orientation_weight (cost/rad of EE orientation error in the anchor heuristic, default "0"),
#             expansion_threads (edge checks per expansion, coal only; default: all cores),
#             snap_ik_seeds (concurrent IK seeds per snap attempt on pose goals, default "1"),
#             graph_growth ("on_stall" default / "upfront"), stall_window (default "50"), root_interval ("0"),
#             root_selection ("frontier_nearest" default / "sequential")
#   heuristic_<articulation>, mprim_path_<articulation>  (per-agent, multi-agent only)
#   weight_low_level_heuristic (applies "50.0" when omitted, though it logs 1.0),
#   high_level_focal_suboptimality (default 1.3),
#   low_level_focal_suboptimality (default 1.3)
#   verbose ("true"/"1" for search progress diagnostics, default "false")
#   validity_backend: "coal" (default) / "vamp" (raises if the robot has no matching module) /
#               "auto" (VAMP when every planned robot supports it, else coal; never raises)
#   VAMP scene translation (meters unless noted): mesh_sampling_resolution (default "0.01"),
#     pointcloud_point_radius ("0.01"), articulation_link_sampling_resolution ("0.05"),
#     attachment_spherization_resolution ("0.05"), attachment_max_spheres ("512"; an attached
#     mesh over the budget raises -- raise it, or attach a primitive like add_box instead)

# === Kinematics: ee_pose and plan_screw ===
# compute_ik(articulation_name, ee_pose, init_state_val): ee_pose is a Pose or a flat
#   [x, y, z, roll, pitch, yaw] list, in the WORLD frame (the frame compute_fk returns; the robot's
#   base pose is applied).
# plan_screw(articulation_name, link_name, ...): Cartesian screw-motion plan via Jacobian
#   resolved-rate stepping (no IK solve); each step is collision- and joint-limit-checked.
#   Give exactly one of:
#     - end_pose: target pose for link_name (axis, pitch and angle are derived)
#     - axis_direction + angle (+ optional axis_point, pitch): explicit screw motion, e.g. a
#       hinge or door swing. axis_point defaults to link_name's current position.
#   start_qpos defaults to the articulation's current qpos and is also set as a side effect.
#   Raises RuntimeError on inconsistent parameters, zero relative rotation, collision, a joint-limit
#   violation, a singularity stall, or exceeding max_steps.

# === Collision backends ===
# is_state_colliding(...) returns True for a COLLISION. is_vamp_state_valid(...) is the VAMP
#   counterpart and has the opposite polarity: True means a VALID configuration.
# is_robot_vamp_supported(name) is False also when a VAMP module of that name exists but does not
#   match this robot's kinematics (e.g. VAMP's built-in ur5 vs the registry ur5); that robot then
#   plans on coal.

# === Visualizer ===
# start_visualizer(type="viser", **kwargs): the preferred way to get a live 3D viewer. It constructs
#   and attaches the viewer, opens the browser, and returns the instance. type: "viser" or "meshcat".
#   If a viser viewer is already attached and running, that one is returned (pass port=... only to
#   deliberately open a second viewer). e.g. viz = planner.start_visualizer("viser")
#   The viser viewer exposes:
#     viz.animate_trajectory(trajectories, dt=0.05, robot_name=None, fps=30.0) -> None
#     viz.stop() -> None
# get_visualizer(index=0): an already-attached visualizer, or None.
# attach_visualizer(listener) / detach_visualizer(listener): low-level, manually attach or detach a
#   VisualizerListener.

# === VAMP plugins ===
# VAMP modules for more robots, without recompiling srmp. add_robot("<registry name>") installs and
#   loads a published prebuilt plugin automatically when one exists for this srmp build (first time:
#   a download plus a short agreement test, up to a few minutes).
srmp.vamp_plugins.plugin_dirs() -> List[Path]   # $SRMP_VAMP_PLUGIN_PATH, then ~/.cache/srmp/vamp_plugins
srmp.vamp_plugins.load_plugins(warn=True, dirs=None) -> List[Dict[str, str]]  # (re)load installed plugins; one report per file
# Building a plugin for a new robot is a shell command, not Python:
#   srmp vamp add-robot --urdf <robot.urdf> ...   |   srmp vamp list | fetch <name> | remove <name> | rebuild-all

# === Robot Registry ===
srmp.robots.list_available(fetch=True) -> Dict[str, List[str]]  # {'remote': [...], 'local': [...], 'custom': [...]}
srmp.robots.download(name, force=False) -> Path       # downloads and caches; returns the robot's cache directory
srmp.robots.download_all(force=False) -> Path         # download every registry robot at once
srmp.robots.get(name) -> RobotInfo   # .name, .urdf_path, .srdf_path, .end_effector, .description, .default_qpos, .joint_names, .gripper_joint_names, .source
srmp.robots.info(name) -> RobotInfo  # alias for get()
srmp.robots.register(name, urdf_path, srdf_path, end_effector, description=None, default_qpos=None,
                     joint_names=None, gripper_joint_names=None) -> None  # register a custom local robot
srmp.robots.unregister(name) -> None
srmp.robots.get_cache_dir() -> Path
srmp.robots.set_cache_dir(path) -> None
# Raises srmp.robots.RobotNotFoundError / srmp.robots.DownloadError on failure
