Cad Viewer

interface:
  display_name: "CAD Viewer"
  short_description: "Open CAD, G-code, and robot files in Viewer."
  default_prompt: "Use $cad-viewer to start CAD Viewer and return review links for explicit CAD, G-code, or robot-description files."

MIT License

Copyright (c) 2026 earthtojake

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

--editable ./scripts/viewer/packages/cadpy

node_modules
.env
.env.*
!.env.example
!.env.*.example
__pycache__
*.py[cod]
.pytest_cache
tmp

!dist
!dist/**

from pathlib import Path

__path__.append(str(Path(__file__).resolve().parent / "moveit2_server"))

#!/usr/bin/env bash
set -euo pipefail

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

# shellcheck source=conda-common.sh
source "${SCRIPT_DIR}/conda-common.sh"

REPO_ROOT="$(find_repo_root)"
CONDA="$(find_conda)"
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_cyclonedds_cpp}"
export PYTHONPATH="${SCRIPT_DIR}${PYTHONPATH:+:${PYTHONPATH}}"

"${CONDA}" run --no-capture-output -n "${MOVEIT2_SERVER_CONDA_ENV_NAME}" python -m moveit2_server.server --check --repo-root "${REPO_ROOT}"

#!/usr/bin/env bash

MOVEIT2_SERVER_CONDA_ENV_NAME="${MOVEIT2_SERVER_CONDA_ENV_NAME:-moveit2-server}"

find_conda() {
  if [[ -n "${MOVEIT2_SERVER_CONDA_EXE:-}" && -x "${MOVEIT2_SERVER_CONDA_EXE}" ]]; then
    printf '%s\n' "${MOVEIT2_SERVER_CONDA_EXE}"
    return 0
  fi
  if [[ -n "${CONDA_EXE:-}" && -x "${CONDA_EXE}" ]]; then
    printf '%s\n' "${CONDA_EXE}"
    return 0
  fi
  if command -v conda >/dev/null 2>&1; then
    command -v conda
    return 0
  fi
  local candidate
  for candidate in \
    "${HOME}/miniforge3/bin/conda" \
    "${HOME}/mambaforge/bin/conda" \
    "${HOME}/miniconda3/bin/conda" \
    "${HOME}/anaconda3/bin/conda" \
    "/opt/conda/bin/conda"
  do
    if [[ -x "${candidate}" ]]; then
      printf '%s\n' "${candidate}"
      return 0
    fi
  done
  printf 'Could not find conda. Install Miniforge, put conda on PATH, or set MOVEIT2_SERVER_CONDA_EXE/CONDA_EXE.\n' >&2
  return 1
}

find_repo_root() {
  if [[ -n "${MOVEIT2_SERVER_REPO_ROOT:-}" ]]; then
    if [[ ! -d "${MOVEIT2_SERVER_REPO_ROOT}" ]]; then
      printf 'MOVEIT2_SERVER_REPO_ROOT does not exist: %s\n' "${MOVEIT2_SERVER_REPO_ROOT}" >&2
      return 1
    fi
    (cd "${MOVEIT2_SERVER_REPO_ROOT}" && pwd -P)
    return 0
  fi

  local root
  if root="$(git -C "${PWD}" rev-parse --show-toplevel 2>/dev/null)"; then
    printf '%s\n' "${root}"
    return 0
  fi

  printf 'Could not find a repository root from the current directory. Run from the robot project repo or set MOVEIT2_SERVER_REPO_ROOT.\n' >&2
  return 1
}

name: moveit2-server
channels:
  - conda-forge
  - robostack-jazzy
dependencies:
  - python=3.12
  - pip
  - websockets
  - ros-jazzy-ros-base
  - ros-jazzy-rclpy
  - ros-jazzy-geometry-msgs
  - ros-jazzy-moveit-py
  - ros-jazzy-moveit-planners-ompl
  - ros-jazzy-rmw-cyclonedds-cpp

"""MoveIt2 websocket server for SRDF workflows."""

from __future__ import annotations

from math import isfinite
import posixpath
import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Any

from moveit2_server.protocol import MotionProtocolError, SUPPORTED_REQUEST_TYPES
from moveit2_server.srdf_source import SrdfSource, read_srdf_source


DEFAULT_CAD_DIRECTORY = ""
DEFAULT_IK_TIMEOUT = 0.05
DEFAULT_IK_ATTEMPTS = 1
DEFAULT_POSITION_TOLERANCE = 0.002
DEFAULT_PLANNING_PIPELINE = "ompl"
DEFAULT_PLANNER_ID = "RRTConnectkConfigDefault"
DEFAULT_PLANNING_TIME = 1.0


def _plain_object(value: Any) -> bool:
    return isinstance(value, dict)


def _path_is_inside(child_path: Path, parent_path: Path) -> bool:
    try:
        child_path.resolve().relative_to(parent_path.resolve())
        return True
    except ValueError:
        return False


def _file_version(file_path: Path) -> str:
    try:
        stats = file_path.stat()
    except FileNotFoundError:
        return ""
    if not stats.st_mode:
        return ""
    return f"{stats.st_size:x}-{stats.st_mtime_ns:x}"


def _combined_version(paths: list[Path]) -> str:
    return "|".join(f"{path.name}:{_file_version(path)}" for path in paths)


def _normalize_relative_path(value: Any, *, label: str, suffix: str | None = None) -> str:
    raw_value = str(value or "").strip().replace("\\", "/").lstrip("/")
    normalized = posixpath.normpath(raw_value)
    if not raw_value or normalized in {"", ".", ".."} or normalized.startswith("../"):
        raise MotionProtocolError(f"{label} must stay inside the repository: {raw_value or '(missing)'}")
    if suffix and not normalized.lower().endswith(suffix):
        raise MotionProtocolError(f"{label} must end in {suffix}: {normalized}")
    return normalized


def normalize_cad_directory(value: Any = DEFAULT_CAD_DIRECTORY) -> str:
    if value is None:
        value = DEFAULT_CAD_DIRECTORY
    raw_value = str(value).strip()
    if not raw_value:
        return ""
    return _normalize_relative_path(raw_value, label="dir")


def normalize_file_ref(value: Any) -> str:
    return _normalize_relative_path(value, label="file", suffix=".srdf")


def _file_ref_relative_to_cad_dir(file_ref: str, *, cad_dir: str, cad_root: Path) -> str:
    if not cad_dir:
        return file_ref
    prefix = f"{cad_dir.rstrip('/')}/"
    if not file_ref.startswith(prefix):
        return file_ref
    scan_relative_ref = file_ref[len(prefix):]
    if scan_relative_ref and not (cad_root / file_ref).is_file() and (cad_root / scan_relative_ref).is_file():
        return scan_relative_ref
    return file_ref


def _resolve_srdf_urdf_path(srdf_source: SrdfSource, *, srdf_path: Path, repo_root: Path) -> Path:
    raw_value = str(srdf_source.urdf_ref or "").strip()
    if not raw_value:
        raise MotionProtocolError("SRDF must include tcad:urdf metadata")
    if "\\" in raw_value:
        raise MotionProtocolError("SRDF tcad:urdf path must use POSIX '/' separators")
    raw_path = Path(raw_value)
    if raw_path.is_absolute():
        raise MotionProtocolError("SRDF tcad:urdf path must be relative")
    candidate = (srdf_path.parent / raw_path).resolve()
    if not _path_is_inside(candidate, repo_root):
        raise MotionProtocolError(f"SRDF tcad:urdf path must stay inside the repository: {raw_value}")
    if candidate.suffix.lower() != ".urdf":
        raise MotionProtocolError(f"SRDF tcad:urdf path must end in .urdf: {raw_value}")
    if not candidate.is_file():
        raise MotionProtocolError(f"SRDF tcad:urdf file does not exist: {raw_value}")
    return candidate


def _urdf_robot(urdf_path: Path) -> dict[str, Any]:
    try:
        root = ET.parse(urdf_path).getroot()
    except FileNotFoundError as exc:
        raise MotionProtocolError(f"URDF file does not exist: {urdf_path}") from exc
    except ET.ParseError as exc:
        raise MotionProtocolError(f"URDF file is invalid XML: {urdf_path}") from exc
    if root.tag != "robot":
        raise MotionProtocolError("URDF root must be <robot>")
    links = {
        str(link.get("name") or "").strip()
        for link in root.findall("link")
        if str(link.get("name") or "").strip()
    }
    joints: dict[str, dict[str, str]] = {}
    for joint in root.findall("joint"):
        name = str(joint.get("name") or "").strip()
        if not name:
            continue
        parent_element = joint.find("parent")
        child_element = joint.find("child")
        joint_type = str(joint.get("type") or "").strip()
        limit_element = joint.find("limit")
        lower = _optional_finite_float(limit_element.get("lower") if limit_element is not None else None)
        upper = _optional_finite_float(limit_element.get("upper") if limit_element is not None else None)
        joints[name] = {
            "type": joint_type,
            "parent": str(parent_element.get("link") if parent_element is not None else "").strip(),
            "child": str(child_element.get("link") if child_element is not None else "").strip(),
            "lower": lower if joint_type in {"revolute", "prismatic"} else None,
            "upper": upper if joint_type in {"revolute", "prismatic"} else None,
            "mimic": joint.find("mimic") is not None,
        }
    return {
        "name": str(root.get("name") or "").strip(),
        "links": links,
        "joints": joints,
    }


def _optional_finite_float(value: Any) -> float | None:
    if value is None:
        return None
    try:
        parsed = float(value)
    except (TypeError, ValueError):
        return None
    return parsed if isfinite(parsed) else None


def _active_joint_names(robot: dict[str, Any]) -> list[str]:
    joints = robot.get("joints")
    if not isinstance(joints, dict):
        return []
    return [
        str(name)
        for name, joint in joints.items()
        if isinstance(joint, dict) and str(joint.get("type") or "").strip() != "fixed" and not bool(joint.get("mimic"))
    ]


def _all_joint_names_for_chain(robot: dict[str, Any], *, base_link: str, tip_link: str) -> list[str]:
    if not base_link or not tip_link:
        return []
    if base_link == tip_link:
        return []
    joints = robot.get("joints")
    if not isinstance(joints, dict):
        return []
    by_parent: dict[str, list[tuple[str, dict[str, Any]]]] = {}
    for raw_name, raw_joint in joints.items():
        if not isinstance(raw_joint, dict):
            continue
        parent = str(raw_joint.get("parent") or "").strip()
        child = str(raw_joint.get("child") or "").strip()
        if parent and child:
            by_parent.setdefault(parent, []).append((str(raw_name), raw_joint))

    stack: list[tuple[str, list[tuple[str, dict[str, Any]]]]] = [(base_link, [])]
    visited: set[str] = set()
    while stack:
        link_name, path = stack.pop()
        if link_name == tip_link:
            return [joint_name for joint_name, _joint in path]
        if link_name in visited:
            continue
        visited.add(link_name)
        for joint_name, joint in reversed(by_parent.get(link_name, [])):
            child = str(joint.get("child") or "").strip()
            if child:
                stack.append((child, [*path, (joint_name, joint)]))
    return []


def _joint_names_for_chain(robot: dict[str, Any], *, base_link: str, tip_link: str) -> list[str]:
    joints = robot.get("joints")
    if not isinstance(joints, dict):
        return []
    names: list[str] = []
    for joint_name in _all_joint_names_for_chain(robot, base_link=base_link, tip_link=tip_link):
        joint = joints.get(joint_name)
        if isinstance(joint, dict) and str(joint.get("type") or "").strip() != "fixed" and not bool(joint.get("mimic")):
            names.append(joint_name)
    return names


def _append_unique(target: list[str], values: list[str]) -> None:
    seen = set(target)
    for value in values:
        if value not in seen:
            target.append(value)
            seen.add(value)


def _joint_names_for_group(
    group: dict[str, Any],
    *,
    robot: dict[str, Any],
    groups_by_name: dict[str, dict[str, Any]],
    visited: set[str] | None = None,
) -> list[str]:
    explicit_joint_names = list(group.get("jointNames") or [])
    if explicit_joint_names:
        joints = robot.get("joints")
        if not isinstance(joints, dict):
            return []
        return [
            str(name)
            for name in explicit_joint_names
            if isinstance(joints.get(str(name)), dict)
            and str(joints[str(name)].get("type") or "").strip() != "fixed"
            and not bool(joints[str(name)].get("mimic"))
        ]

    names: list[str] = []
    for chain in group.get("chains") or []:
        if not isinstance(chain, dict):
            continue
        _append_unique(
            names,
            _joint_names_for_chain(
                robot,
                base_link=str(chain.get("baseLink") or "").strip(),
                tip_link=str(chain.get("tipLink") or "").strip(),
            ),
        )

    if visited is None:
        visited = set()
    group_name = str(group.get("name") or "").strip()
    if group_name:
        visited.add(group_name)
    for subgroup_name in group.get("subgroups") or []:
        subgroup_key = str(subgroup_name or "").strip()
        if not subgroup_key or subgroup_key in visited:
            continue
        subgroup = groups_by_name.get(subgroup_key)
        if subgroup is not None:
            _append_unique(
                names,
                _joint_names_for_group(
                    subgroup,
                    robot=robot,
                    groups_by_name=groups_by_name,
                    visited=visited,
                ),
            )

    return names


def _positive_float(value: Any, *, label: str, default: float) -> float:
    if value is None:
        value = default
    try:
        numeric = float(value)
    except (TypeError, ValueError) as exc:
        raise MotionProtocolError(f"{label} must be positive") from exc
    if numeric <= 0:
        raise MotionProtocolError(f"{label} must be positive")
    return numeric


def _positive_int(value: Any, *, label: str, default: int) -> int:
    if value is None:
        value = default
    try:
        numeric = int(value)
    except (TypeError, ValueError) as exc:
        raise MotionProtocolError(f"{label} must be a positive integer") from exc
    if numeric <= 0:
        raise MotionProtocolError(f"{label} must be a positive integer")
    return numeric


def _scaling(value: Any, *, label: str, default: float) -> float:
    if value is None:
        value = default
    try:
        numeric = float(value)
    except (TypeError, ValueError) as exc:
        raise MotionProtocolError(f"{label} must be between 0 and 1") from exc
    if numeric <= 0 or numeric > 1:
        raise MotionProtocolError(f"{label} must be between 0 and 1")
    return numeric


def _boolean(value: Any, *, default: bool) -> bool:
    if value is None:
        return default
    if isinstance(value, bool):
        return value
    if isinstance(value, str):
        normalized = value.strip().lower()
        if normalized in {"1", "true", "yes", "on"}:
            return True
        if normalized in {"0", "false", "no", "off"}:
            return False
    return bool(value)


def _string(value: Any, *, label: str, default: str = "") -> str:
    normalized = str(value if value is not None else default).strip()
    if not normalized:
        raise MotionProtocolError(f"{label} is required")
    return normalized


def _optional_object(value: Any, *, label: str) -> dict[str, Any]:
    if value is None:
        return {}
    if not _plain_object(value):
        raise MotionProtocolError(f"{label} must be an object")
    return value


def _validate_srdf_inventory(metadata: dict[str, Any], *, robot: dict[str, Any]) -> dict[str, Any]:
    if str(metadata.get("robotName") or "").strip() != str(robot.get("name") or "").strip():
        raise MotionProtocolError("SRDF robotName must match the linked URDF robot name")

    link_names = robot.get("links")
    joint_payload = robot.get("joints")
    if not isinstance(link_names, set) or not isinstance(joint_payload, dict):
        raise MotionProtocolError("URDF robot inventory is invalid")
    joint_names = set(joint_payload)

    groups = _validate_planning_groups(metadata.get("planningGroups"), link_names=link_names, joint_names=joint_names)
    group_by_name = {group["name"]: group for group in groups}
    end_effectors = _validate_end_effectors(
        metadata.get("endEffectors"),
        link_names=link_names,
        groups=group_by_name,
        robot=robot,
    )
    group_states = _validate_group_states(metadata.get("groupStates"), groups=group_by_name, robot=robot)
    disabled_pairs = _validate_disabled_pairs(metadata.get("disabledCollisionPairs"), link_names=link_names)
    return {
        **metadata,
        "planningGroups": groups,
        "endEffectors": end_effectors,
        "groupStates": group_states,
        "disabledCollisionPairs": disabled_pairs,
    }


def _group_tip_link(group: Any) -> str:
    if isinstance(group, dict):
        link_names = group.get("linkNames")
        if isinstance(link_names, list) and link_names:
            return str(link_names[-1])
        chains = group.get("chains")
        if isinstance(chains, list) and chains:
            chain = chains[-1]
            if isinstance(chain, dict):
                return str(chain.get("tipLink") or "")
        return ""
    link_names = getattr(group, "link_names", ())
    if link_names:
        return str(link_names[-1])
    chains = getattr(group, "chains", ())
    if chains:
        return str(getattr(chains[-1], "tip_link", "") or "")
    return ""


def _end_effector_link(end_effector: Any, groups_by_name: dict[str, Any]) -> str:
    group_name = str(getattr(end_effector, "group", "") or "")
    group = groups_by_name.get(group_name)
    if group is not None:
        group_tip = _group_tip_link(group)
        if group_tip:
            return group_tip
    return ""


def _srdf_inventory_from_source(srdf_source: SrdfSource, *, srdf_path: Path, urdf_path: Path) -> dict[str, Any]:
    groups_by_name = {group.name: group for group in srdf_source.planning_groups}
    return {
        "urdf": str(urdf_path),
        "srdf": str(srdf_path),
        "robotName": srdf_source.robot_name,
        "planningGroups": [
            {
                "name": group.name,
                "jointNames": list(group.joint_names),
                "linkNames": list(group.link_names),
                "chains": [
                    {"baseLink": chain.base_link, "tipLink": chain.tip_link}
                    for chain in group.chains
                ],
                "subgroups": list(group.subgroups),
            }
            for group in srdf_source.planning_groups
        ],
        "endEffectors": [
            {
                "name": end_effector.name,
                "link": _end_effector_link(end_effector, groups_by_name) or end_effector.parent_link,
                "parentLink": end_effector.parent_link,
                "group": end_effector.group,
                "parentGroup": end_effector.parent_group,
            }
            for end_effector in srdf_source.end_effectors
        ],
        "groupStates": [
            {
                "name": group_state.name,
                "group": group_state.group,
                "jointValuesByName": group_state.joint_values_by_name,
                "jointValuesByNameRad": group_state.joint_values_by_name,
            }
            for group_state in srdf_source.group_states
        ],
        "disabledCollisionPairs": [
            {
                "link1": pair.link1,
                "link2": pair.link2,
                "reason": pair.reason,
                "source": pair.source,
            }
            for pair in srdf_source.disabled_collision_pairs
        ],
    }


def _validate_planning_groups(value: Any, *, link_names: set[str], joint_names: set[str]) -> list[dict[str, Any]]:
    if not isinstance(value, list) or not value:
        raise MotionProtocolError("SRDF planningGroups must be a non-empty array")
    groups: list[dict[str, Any]] = []
    seen = set()
    for raw_group in value:
        if not _plain_object(raw_group):
            raise MotionProtocolError("SRDF planning group must be an object")
        name = _string(raw_group.get("name"), label="SRDF planning group name")
        if name in seen:
            raise MotionProtocolError(f"Duplicate SRDF planning group {name}")
        seen.add(name)
        group_joint_names = _validate_names(raw_group.get("jointNames"), allowed=joint_names, label=f"SRDF planning group {name} jointNames")
        group_link_names = _validate_names(raw_group.get("linkNames"), allowed=link_names, label=f"SRDF planning group {name} linkNames")
        chains = raw_group.get("chains") if isinstance(raw_group.get("chains"), list) else []
        normalized_chains: list[dict[str, str]] = []
        for raw_chain in chains:
            if not _plain_object(raw_chain):
                raise MotionProtocolError(f"SRDF planning group {name} chains must contain objects")
            base_link = _string(raw_chain.get("baseLink"), label=f"SRDF planning group {name} chain baseLink")
            tip_link = _string(raw_chain.get("tipLink"), label=f"SRDF planning group {name} chain tipLink")
            if base_link not in link_names or tip_link not in link_names:
                raise MotionProtocolError(f"SRDF planning group {name} chain references missing URDF link")
            normalized_chains.append({"baseLink": base_link, "tipLink": tip_link})
        subgroups = [str(group).strip() for group in raw_group.get("subgroups", []) if str(group).strip()]
        groups.append({
            "name": name,
            "jointNames": group_joint_names,
            "linkNames": group_link_names,
            "chains": normalized_chains,
            "subgroups": subgroups,
        })
    group_names = {group["name"] for group in groups}
    for group in groups:
        for subgroup in group["subgroups"]:
            if subgroup not in group_names:
                raise MotionProtocolError(f"SRDF planning group {group['name']} references missing subgroup {subgroup}")
    return groups


def _validate_names(value: Any, *, allowed: set[str], label: str) -> list[str]:
    if value is None:
        return []
    if not isinstance(value, list):
        raise MotionProtocolError(f"{label} must be an array")
    names: list[str] = []
    seen = set()
    for raw_name in value:
        name = str(raw_name or "").strip()
        if not name:
            raise MotionProtocolError(f"{label} cannot include empty names")
        if name in seen:
            raise MotionProtocolError(f"{label} includes duplicate {name}")
        if name not in allowed:
            raise MotionProtocolError(f"{label} references missing name {name}")
        seen.add(name)
        names.append(name)
    return names


def _link_names_for_group(
    group: dict[str, Any],
    *,
    robot: dict[str, Any],
    groups_by_name: dict[str, dict[str, Any]],
    visited: set[str] | None = None,
) -> set[str]:
    links = set(str(link) for link in group.get("linkNames", []) if str(link))
    joints = robot.get("joints")
    if not isinstance(joints, dict):
        return links

    def add_joint_links(joint_name: str) -> None:
        joint = joints.get(joint_name)
        if not isinstance(joint, dict):
            return
        child = str(joint.get("child") or "").strip()
        if child:
            links.add(child)

    for joint_name in group.get("jointNames", []):
        add_joint_links(str(joint_name))
    for chain in group.get("chains", []):
        if not isinstance(chain, dict):
            continue
        base_link = str(chain.get("baseLink") or "").strip()
        tip_link = str(chain.get("tipLink") or "").strip()
        if tip_link:
            links.add(tip_link)
        for joint_name in _all_joint_names_for_chain(robot, base_link=base_link, tip_link=tip_link):
            add_joint_links(joint_name)

    if visited is None:
        visited = set()
    group_name = str(group.get("name") or "").strip()
    if group_name:
        visited.add(group_name)
    for subgroup_name in group.get("subgroups", []):
        subgroup_key = str(subgroup_name or "").strip()
        if not subgroup_key or subgroup_key in visited:
            continue
        subgroup = groups_by_name.get(subgroup_key)
        if subgroup is not None:
            links.update(_link_names_for_group(subgroup, robot=robot, groups_by_name=groups_by_name, visited=visited))
    links.discard("")
    return links


def _joint_adjacent_to_any_link(robot: dict[str, Any], parent_link: str, child_links: set[str]) -> bool:
    joints = robot.get("joints")
    if not isinstance(joints, dict):
        return False
    for joint in joints.values():
        if not isinstance(joint, dict):
            continue
        parent = str(joint.get("parent") or "").strip()
        child = str(joint.get("child") or "").strip()
        if (parent == parent_link and child in child_links) or (child == parent_link and parent in child_links):
            return True
    return False


def _validate_end_effectors(
    value: Any,
    *,
    link_names: set[str],
    groups: dict[str, dict[str, Any]],
    robot: dict[str, Any],
) -> list[dict[str, Any]]:
    if value is None:
        return []
    if not isinstance(value, list):
        raise MotionProtocolError("SRDF endEffectors must be an array")
    end_effectors: list[dict[str, Any]] = []
    seen = set()
    for raw_end_effector in value:
        if not _plain_object(raw_end_effector):
            raise MotionProtocolError("SRDF end effector must be an object")
        name = _string(raw_end_effector.get("name"), label="SRDF end effector name")
        if name in seen:
            raise MotionProtocolError(f"Duplicate SRDF end effector {name}")
        seen.add(name)
        parent_link = _string(raw_end_effector.get("parentLink"), label=f"SRDF end effector {name} parentLink")
        if parent_link not in link_names:
            raise MotionProtocolError(f"SRDF end effector {name} references missing parentLink {parent_link}")
        group_name = _string(raw_end_effector.get("group"), label=f"SRDF end effector {name} group")
        if group_name not in groups:
            raise MotionProtocolError(f"SRDF end effector {name} references missing group {group_name}")
        parent_group = str(raw_end_effector.get("parentGroup") or "").strip()
        if parent_group and parent_group not in groups:
            raise MotionProtocolError(f"SRDF end effector {name} references missing parentGroup {parent_group}")
        link = str(raw_end_effector.get("link") or _group_tip_link(groups[group_name]) or parent_link).strip()
        if link not in link_names:
            raise MotionProtocolError(f"SRDF end effector {name} references missing link {link}")
        end_effector_links = _link_names_for_group(groups[group_name], robot=robot, groups_by_name=groups)
        if parent_group:
            parent_group_links = _link_names_for_group(groups[parent_group], robot=robot, groups_by_name=groups)
            overlap = sorted(end_effector_links & parent_group_links)
            if overlap:
                raise MotionProtocolError(f"SRDF end effector {name} group shares link(s) with parentGroup: {overlap}")
            if parent_link not in parent_group_links:
                raise MotionProtocolError(
                    f"SRDF end effector {name} parentLink {parent_link} is not in parentGroup {parent_group}"
                )
        if end_effector_links and parent_link not in end_effector_links and not _joint_adjacent_to_any_link(
            robot,
            parent_link,
            end_effector_links,
        ):
            raise MotionProtocolError(f"SRDF end effector {name} parentLink is not adjacent to its group")
        end_effectors.append({
            "name": name,
            "link": link,
            "parentLink": parent_link,
            "group": group_name,
            "parentGroup": parent_group,
        })
    return end_effectors


def _validate_group_state_joint_value(state_name: str, joint_name: str, value: float, joint: dict[str, Any]) -> None:
    joint_type = str(joint.get("type") or "").strip()
    if joint_type == "fixed":
        raise MotionProtocolError(f"SRDF group state {state_name} cannot set fixed joint {joint_name}")
    if bool(joint.get("mimic")):
        raise MotionProtocolError(f"SRDF group state {state_name} cannot set mimic joint {joint_name}")
    if joint_type == "continuous":
        return
    lower = joint.get("lower")
    upper = joint.get("upper")
    if isinstance(lower, float) and value < lower:
        raise MotionProtocolError(f"SRDF group state {state_name} joint {joint_name} is below its URDF lower limit")
    if isinstance(upper, float) and value > upper:
        raise MotionProtocolError(f"SRDF group state {state_name} joint {joint_name} is above its URDF upper limit")


def _validate_group_states(value: Any, *, groups: dict[str, dict[str, Any]], robot: dict[str, Any]) -> list[dict[str, Any]]:
    if value is None:
        return []
    if not isinstance(value, list):
        raise MotionProtocolError("SRDF groupStates must be an array")
    states: list[dict[str, Any]] = []
    seen_keys: set[str] = set()
    joint_payload = robot.get("joints")
    if not isinstance(joint_payload, dict):
        raise MotionProtocolError("URDF robot inventory is invalid")
    for raw_state in value:
        if not _plain_object(raw_state):
            raise MotionProtocolError("SRDF group state must be an object")
        name = _string(raw_state.get("name"), label="SRDF group state name")
        group = _string(raw_state.get("group"), label=f"SRDF group state {name} group")
        state_key = f"{group}/{name}"
        if state_key in seen_keys:
            raise MotionProtocolError(f"Duplicate SRDF group state {state_key}")
        seen_keys.add(state_key)
        if group not in groups:
            raise MotionProtocolError(f"SRDF group state {name} references missing group {group}")
        group_joint_names = set(_joint_names_for_group(groups[group], robot=robot, groups_by_name=groups))
        joint_values = raw_state.get("jointValuesByName")
        legacy_joint_values = raw_state.get("jointValuesByNameRad")
        if joint_values is None and legacy_joint_values is not None:
            joint_values = legacy_joint_values
        if not _plain_object(joint_values):
            raise MotionProtocolError(f"SRDF group state {name} jointValuesByName must be an object")
        parsed_values: dict[str, float] = {}
        for raw_joint_name, raw_value in joint_values.items():
            joint_name = str(raw_joint_name or "").strip()
            joint = joint_payload.get(joint_name)
            if not isinstance(joint, dict):
                raise MotionProtocolError(f"SRDF group state {name} references missing joint {joint_name}")
            if joint_name not in group_joint_names:
                raise MotionProtocolError(f"SRDF group state {name} joint {joint_name} is not in group {group}")
            try:
                parsed_value = float(raw_value)
            except (TypeError, ValueError) as exc:
                raise MotionProtocolError(f"SRDF group state {name} joint {joint_name} must be numeric") from exc
            if not isfinite(parsed_value):
                raise MotionProtocolError(f"SRDF group state {name} joint {joint_name} must be finite")
            _validate_group_state_joint_value(name, joint_name, parsed_value, joint)
            parsed_values[joint_name] = parsed_value
        states.append({"name": name, "group": group, "jointValuesByName": parsed_values, "jointValuesByNameRad": parsed_values})
    return states


def _validate_disabled_pairs(value: Any, *, link_names: set[str]) -> list[dict[str, str]]:
    if value is None:
        return []
    if not isinstance(value, list):
        raise MotionProtocolError("SRDF disabledCollisionPairs must be an array")
    pairs: list[dict[str, str]] = []
    seen: set[str] = set()
    for raw_pair in value:
        if not _plain_object(raw_pair):
            raise MotionProtocolError("SRDF disabled collision pair must be an object")
        link1 = _string(raw_pair.get("link1"), label="SRDF disabled collision link1")
        link2 = _string(raw_pair.get("link2"), label="SRDF disabled collision link2")
        if link1 == link2:
            raise MotionProtocolError("SRDF disabled collision pair cannot repeat the same link")
        if link1 not in link_names or link2 not in link_names:
            raise MotionProtocolError("SRDF disabled collision pair references missing URDF link")
        pair_key = "/".join(sorted((link1, link2)))
        if pair_key in seen:
            raise MotionProtocolError(f"Duplicate SRDF disabled collision pair {pair_key}")
        seen.add(pair_key)
        reason = str(raw_pair.get("reason") or "").strip()
        if not reason:
            raise MotionProtocolError(f"SRDF disabled collision pair {pair_key} requires a reason")
        pairs.append({
            "link1": link1,
            "link2": link2,
            "reason": reason,
            "source": str(raw_pair.get("source") or "manual").strip(),
        })
    return pairs


def _moveit_settings(payload: Any) -> dict[str, Any]:
    if not _plain_object(payload):
        return {}
    return _optional_object(payload.get("moveit2"), label="moveit2")


def _target_payload(payload: Any) -> dict[str, Any]:
    if not _plain_object(payload):
        return {}
    return _optional_object(payload.get("target"), label="target")


def _find_group(metadata: dict[str, Any], name: str) -> dict[str, Any]:
    for group in metadata.get("planningGroups", []):
        if str(group.get("name") or "") == name:
            return group
    raise MotionProtocolError(f"Selected planning group {name} is not defined by the SRDF")


def _find_end_effector(metadata: dict[str, Any], name: str) -> dict[str, Any]:
    for end_effector in metadata.get("endEffectors", []):
        if str(end_effector.get("name") or "") == name:
            return end_effector
    raise MotionProtocolError(f"Selected end effector {name} is not defined by the SRDF")


def _default_end_effector(metadata: dict[str, Any], planning_group_name: str) -> dict[str, Any] | None:
    end_effectors = list(metadata.get("endEffectors", []))
    for end_effector in end_effectors:
        if str(end_effector.get("parentGroup") or "") == planning_group_name:
            return end_effector
    return end_effectors[0] if end_effectors else None


def _build_command(
    *,
    command_name: str,
    payload: Any,
    metadata: dict[str, Any],
    robot: dict[str, Any],
) -> dict[str, Any]:
    moveit2 = _moveit_settings(payload)
    target = _target_payload(payload)
    default_group = metadata["planningGroups"][0]
    planning_group_name = _string(
        moveit2.get("planningGroup"),
        label="moveit2.planningGroup",
        default=str(default_group.get("name") or ""),
    )
    planning_group = _find_group(metadata, planning_group_name)

    default_end_effector = _default_end_effector(metadata, planning_group_name)
    selected_end_effector_name = str(moveit2.get("endEffector") or target.get("endEffector") or "").strip()
    selected_end_effector = _find_end_effector(metadata, selected_end_effector_name) if selected_end_effector_name else default_end_effector
    if selected_end_effector is None:
        raise MotionProtocolError("SRDF must define an end effector for MoveIt2 pose requests")

    robot_links = robot.get("links")
    if not isinstance(robot_links, set):
        raise MotionProtocolError("URDF robot inventory is invalid")
    target_frame = _string(
        moveit2.get("targetFrame") or target.get("frame"),
        label="moveit2.targetFrame",
        default=next(iter(sorted(robot_links)), ""),
    )
    if target_frame not in robot_links:
        raise MotionProtocolError(f"moveit2.targetFrame references missing URDF link {target_frame}")

    ik = _optional_object(moveit2.get("ik"), label="moveit2.ik")
    planning = _optional_object(moveit2.get("planning"), label="moveit2.planning")
    target_has_orientation = "quat_xyzw" in target or "rpy" in target
    position_only_ik = _boolean(ik.get("positionOnly"), default=not target_has_orientation)
    if not target_has_orientation and not position_only_ik:
        raise MotionProtocolError("target orientation is required when moveit2.ik.positionOnly is false")
    groups_by_name = {
        str(group.get("name") or ""): group
        for group in metadata.get("planningGroups", [])
        if isinstance(group, dict) and str(group.get("name") or "")
    }
    joint_names = _joint_names_for_group(planning_group, robot=robot, groups_by_name=groups_by_name)
    target_link = _string(
        moveit2.get("targetLink") or target.get("targetLink") or target.get("link"),
        label="moveit2.targetLink",
        default=str(selected_end_effector.get("link") or ""),
    )
    if target_link not in robot_links:
        raise MotionProtocolError(f"moveit2.targetLink references missing URDF link {target_link}")
    command = {
        "planningGroup": planning_group_name,
        "jointNames": joint_names,
        "endEffectors": [
            {
                "name": selected_end_effector["name"],
                "link": target_link,
                "inferredLink": selected_end_effector["link"],
                "frame": target_frame,
                "planningGroup": planning_group_name,
                "jointNames": joint_names,
                "positionTolerance": _positive_float(
                    ik.get("tolerance", moveit2.get("positionTolerance")),
                    label="moveit2.ik.tolerance",
                    default=DEFAULT_POSITION_TOLERANCE,
                ),
            }
        ],
        "ik": {
            "timeout": _positive_float(ik.get("timeout"), label="moveit2.ik.timeout", default=DEFAULT_IK_TIMEOUT),
            "attempts": _positive_int(ik.get("attempts"), label="moveit2.ik.attempts", default=DEFAULT_IK_ATTEMPTS),
            "positionOnly": position_only_ik,
            "tolerance": _positive_float(
                ik.get("tolerance", moveit2.get("positionTolerance")),
                label="moveit2.ik.tolerance",
                default=DEFAULT_POSITION_TOLERANCE,
            ),
        },
        "planner": {
            "pipeline": _string(planning.get("pipeline"), label="moveit2.planning.pipeline", default=DEFAULT_PLANNING_PIPELINE),
            "plannerId": _string(planning.get("plannerId"), label="moveit2.planning.plannerId", default=DEFAULT_PLANNER_ID),
            "planningTime": _positive_float(planning.get("planningTime"), label="moveit2.planning.planningTime", default=DEFAULT_PLANNING_TIME),
            "maxVelocityScalingFactor": _scaling(
                planning.get("maxVelocityScalingFactor"),
                label="moveit2.planning.maxVelocityScalingFactor",
                default=1.0,
            ),
            "maxAccelerationScalingFactor": _scaling(
                planning.get("maxAccelerationScalingFactor"),
                label="moveit2.planning.maxAccelerationScalingFactor",
                default=1.0,
            ),
        },
    }
    if command_name not in SUPPORTED_REQUEST_TYPES:
        raise MotionProtocolError(f"Unsupported request type {command_name}")
    return command


def build_moveit2_context(
    *,
    repo_root: str | Path,
    dir: Any = DEFAULT_CAD_DIRECTORY,
    file: Any,
    type: Any,
    payload: Any = None,
) -> dict[str, Any]:
    command_name = str(type or "").strip()
    if command_name not in SUPPORTED_REQUEST_TYPES:
        raise MotionProtocolError(f"Unsupported request type {command_name or '(missing)'}")
    resolved_repo_root = Path(repo_root).resolve()
    cad_dir = normalize_cad_directory(dir)
    file_ref = normalize_file_ref(file)
    cad_root = (resolved_repo_root / cad_dir).resolve()
    if not _path_is_inside(cad_root, resolved_repo_root):
        raise MotionProtocolError(f"dir must stay inside the repository: {cad_dir}")
    file_ref = _file_ref_relative_to_cad_dir(file_ref, cad_dir=cad_dir, cad_root=cad_root)
    srdf_path = (cad_root / file_ref).resolve()
    if not _path_is_inside(srdf_path, cad_root):
        root_label = cad_dir or "the repository"
        raise MotionProtocolError(f"file must stay inside {root_label}: {file_ref}")
    if not srdf_path.is_file():
        raise MotionProtocolError(f"SRDF file does not exist: {file_ref}")

    try:
        srdf_source = read_srdf_source(srdf_path)
    except Exception as exc:
        raise MotionProtocolError(str(exc)) from exc
    urdf_path = _resolve_srdf_urdf_path(srdf_source, srdf_path=srdf_path, repo_root=resolved_repo_root)
    robot = _urdf_robot(urdf_path)
    inventory_payload = _srdf_inventory_from_source(srdf_source, srdf_path=srdf_path, urdf_path=urdf_path)
    srdf_inventory = _validate_srdf_inventory(inventory_payload, robot=robot)
    command = _build_command(command_name=command_name, payload=payload, metadata=srdf_inventory, robot=robot)
    version_paths = [urdf_path, srdf_path]
    return {
        "repoRoot": str(resolved_repo_root),
        "dir": cad_dir,
        "file": file_ref,
        "urdfPath": str(urdf_path),
        "srdfPath": str(srdf_path),
        "modelAssetHash": _combined_version(version_paths),
        "commandName": command_name,
        "command": command,
        "srdf": srdf_inventory,
    }

from __future__ import annotations

from functools import lru_cache
from typing import Any

from moveit2_server.protocol import MotionProtocolError, MotionRequest
from moveit2_server.moveit_py import MoveItPyAdapter


@lru_cache(maxsize=1)
def _adapter() -> MoveItPyAdapter:
    return MoveItPyAdapter()


def dispatch(request: MotionRequest) -> dict[str, Any]:
    adapter = _adapter()
    if request.type == "srdf.solvePose":
        result = adapter.solve_pose(request)
    elif request.type == "srdf.planToPose":
        result = adapter.plan_to_pose(request)
    else:
        raise MotionProtocolError(f"Unsupported request type {request.type}")

    if not isinstance(result, dict):
        raise MotionProtocolError("MoveIt2 adapter result must be an object")
    if result.get("ok") is False:
        raise RuntimeError(str(result.get("message") or "MoveIt2 request failed"))
    result.pop("ok", None)
    return result

from __future__ import annotations

import argparse
import json
import time
from typing import Any


def _json_list(value: str, label: str) -> list[Any]:
    payload = json.loads(value)
    if not isinstance(payload, list):
        raise ValueError(f"{label} must be a JSON array")
    return payload


def main(argv: list[str] | None = None) -> int:
    parser = argparse.ArgumentParser(prog="python -m moveit2_server.joint_state_seed")
    parser.add_argument("--names", required=True, help="JSON array of joint names")
    parser.add_argument("--positions", required=True, help="JSON array of joint positions in ROS units")
    parser.add_argument("--duration", type=float, default=8.0)
    parser.add_argument("--rate", type=float, default=30.0)
    args = parser.parse_args(argv)

    names = [str(name) for name in _json_list(args.names, "names")]
    positions = [float(position) for position in _json_list(args.positions, "positions")]
    if len(names) != len(positions):
        raise ValueError("names and positions must have the same length")
    if args.duration <= 0 or args.rate <= 0:
        raise ValueError("duration and rate must be positive")

    import rclpy
    from sensor_msgs.msg import JointState

    rclpy.init(args=None)
    node = rclpy.create_node("moveit2_server_joint_state_seed")
    publisher = node.create_publisher(JointState, "/joint_states", 10)
    deadline = time.monotonic() + args.duration
    period = 1.0 / args.rate
    try:
        while time.monotonic() < deadline and rclpy.ok():
            message = JointState()
            message.header.stamp = node.get_clock().now().to_msg()
            message.name = names
            message.position = positions
            publisher.publish(message)
            rclpy.spin_once(node, timeout_sec=0.0)
            time.sleep(period)
    finally:
        try:
            node.destroy_node()
        except Exception:
            pass
        try:
            rclpy.shutdown()
        except Exception:
            pass
    return 0


if __name__ == "__main__":
    raise SystemExit(main())

from __future__ import annotations

import hashlib
import json
import math
from pathlib import Path
import subprocess
import sys
from typing import Any
import xml.etree.ElementTree as ET

from moveit2_server.protocol import (
    MotionProtocolError,
    normalize_joint_values,
    normalize_motion_target,
)


def _deg_to_rad(value: float) -> float:
    return (float(value) * math.pi) / 180.0


def _rad_to_deg(value: float) -> float:
    return (float(value) * 180.0) / math.pi


def _solve_command(request: Any) -> dict[str, Any]:
    return request.command


def _target_end_effector_name(request: Any) -> str:
    target = request.payload.get("target") if isinstance(request.payload, dict) else None
    if not isinstance(target, dict):
        return ""
    return str(target.get("endEffector") or "").strip()


def _selected_end_effector(request: Any) -> dict[str, Any]:
    command = _solve_command(request)
    end_effectors = command.get("endEffectors", [])
    target_name = _target_end_effector_name(request)
    if target_name:
        for end_effector in end_effectors:
            if str(end_effector.get("name", "")).strip() == target_name:
                return end_effector
        raise MotionProtocolError(f"Unknown end effector {target_name}")
    for end_effector in end_effectors:
        return end_effector
    raise MotionProtocolError("moveit_py command endEffectors must be non-empty")


def _joint_names(request: Any) -> list[str]:
    command = _solve_command(request)
    end_effector = _selected_end_effector(request)
    joint_names = [
        str(name).strip()
        for name in end_effector.get("jointNames", command.get("jointNames", []))
        if str(name).strip()
    ]
    if not joint_names:
        raise MotionProtocolError("moveit_py command jointNames must be non-empty")
    return joint_names


def _end_effector(request: Any, name: str) -> dict[str, Any]:
    end_effector = _selected_end_effector(request)
    if str(end_effector.get("name", "")).strip() == name:
        return end_effector
    raise MotionProtocolError(f"Unknown end effector {name}")


def _planning_group(request: Any) -> str:
    command = _solve_command(request)
    end_effector = _selected_end_effector(request)
    planning_group = str(
        end_effector.get("planningGroup")
        or command.get("planningGroup")
        or request.command.get("planningGroup")
        or ""
    ).strip()
    if not planning_group:
        raise MotionProtocolError("moveit_py command planningGroup must be non-empty")
    return planning_group


def _robot_description_for_moveit(urdf_path: Path) -> str:
    """Return URDF XML with relative mesh paths rewritten as file URIs."""
    try:
        tree = ET.parse(urdf_path)
    except ET.ParseError:
        return urdf_path.read_text(encoding="utf-8")
    root = tree.getroot()
    changed = False
    for mesh in root.findall(".//mesh"):
        filename = str(mesh.get("filename") or "").strip()
        if not filename or "://" in filename or filename.startswith("package:"):
            continue
        path = Path(filename)
        if not path.is_absolute():
            path = urdf_path.parent / path
        mesh.set("filename", path.resolve().as_uri())
        changed = True
    if not changed:
        return urdf_path.read_text(encoding="utf-8")
    return ET.tostring(root, encoding="unicode")


def _active_urdf_joints(urdf_path: Path) -> list[tuple[str, str]]:
    try:
        root = ET.parse(urdf_path).getroot()
    except (OSError, ET.ParseError):
        return []
    joints: list[tuple[str, str]] = []
    for joint in root.findall("joint"):
        name = str(joint.get("name") or "").strip()
        joint_type = str(joint.get("type") or "").strip()
        if name and joint_type != "fixed":
            joints.append((name, joint_type))
    return joints


def _urdf_joint_types(urdf_path: Path) -> dict[str, str]:
    try:
        root = ET.parse(urdf_path).getroot()
    except (OSError, ET.ParseError):
        return {}
    joint_types: dict[str, str] = {}
    for joint in root.findall("joint"):
        name = str(joint.get("name") or "").strip()
        joint_type = str(joint.get("type") or "").strip()
        if name:
            joint_types[name] = joint_type
    return joint_types


def _joint_types_for_request(request: Any) -> dict[str, str]:
    urdf_path = Path(str(request.context.get("urdfPath") or ""))
    joint_types = _urdf_joint_types(urdf_path)
    if joint_types:
        return joint_types
    command = _solve_command(request)
    fallback_names = [str(name).strip() for name in command.get("jointNames", []) if str(name).strip()]
    if not fallback_names:
        fallback_names = _joint_names(request)
    return {name: "revolute" for name in fallback_names}


def _native_start_joint_values(request: Any, joint_types: dict[str, str] | None = None) -> dict[str, float]:
    joint_types = joint_types or _joint_types_for_request(request)
    payload = request.payload if isinstance(request.payload, dict) else {}
    if "startJointValuesByName" in payload:
        return normalize_joint_values(payload.get("startJointValuesByName"), label="startJointValuesByName")
    legacy_values = normalize_joint_values(payload.get("startJointValuesByNameDeg"), label="startJointValuesByNameDeg")
    return {
        name: float(value) if str(joint_types.get(name) or "").strip() == "prismatic" else _deg_to_rad(value)
        for name, value in legacy_values.items()
    }


def _native_to_legacy_joint_value(value: float, joint_type: str) -> float:
    return float(value) if str(joint_type or "").strip() == "prismatic" else _rad_to_deg(value)


def _native_to_legacy_joint_values(values: dict[str, float], joint_types: dict[str, str]) -> dict[str, float]:
    return {
        name: _native_to_legacy_joint_value(value, str(joint_types.get(name) or "revolute"))
        for name, value in values.items()
    }


def _moveit2_groups(request: Any) -> list[dict[str, Any]]:
    srdf = request.context.get("srdf", {}) if isinstance(request.context, dict) else {}
    groups = srdf.get("planningGroups", []) if isinstance(srdf, dict) else []
    return [group for group in groups if isinstance(group, dict)]


def _supports_kdl_kinematics(group: dict[str, Any]) -> bool:
    if group.get("subgroups"):
        return False
    return bool(group.get("jointNames") or group.get("chains"))


def _kinematics_config(request: Any) -> dict[str, Any]:
    ik = request.command.get("ik", {}) if isinstance(request.command, dict) else {}
    return {
        str(group.get("name")): {
            "kinematics_solver": "kdl_kinematics_plugin/KDLKinematicsPlugin",
            "kinematics_solver_search_resolution": 0.005,
            "kinematics_solver_timeout": float(ik.get("timeout") or 0.05),
            "position_only_ik": bool(ik.get("positionOnly", True)),
        }
        for group in _moveit2_groups(request)
        if str(group.get("name") or "").strip() and _supports_kdl_kinematics(group)
    }


def _planning_pipeline_config(request: Any) -> dict[str, Any]:
    planner = request.command.get("planner", {}) if isinstance(request.command, dict) else {}
    pipeline = str(planner.get("pipeline") or "ompl")
    planner_id = str(planner.get("plannerId") or "RRTConnectkConfigDefault")
    planning_time = float(planner.get("planningTime") or 1.0)
    def request_params() -> dict[str, Any]:
        return {
            "planning_attempts": int(request.command.get("ik", {}).get("attempts") or 1),
            "planning_pipeline": pipeline,
            "planner_id": planner_id,
            "planning_time": planning_time,
            "max_velocity_scaling_factor": float(planner.get("maxVelocityScalingFactor") or 1.0),
            "max_acceleration_scaling_factor": float(planner.get("maxAccelerationScalingFactor") or 1.0),
        }

    return {
        "planning_pipelines": {
            "pipeline_names": [pipeline],
        },
        pipeline: {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "planning_plugins": ["ompl_interface/OMPLPlanner"],
            "request_adapters": [
                "default_planning_request_adapters/ResolveConstraintFrames",
                "default_planning_request_adapters/ValidateWorkspaceBounds",
                "default_planning_request_adapters/CheckStartStateBounds",
                "default_planning_request_adapters/CheckStartStateCollision",
            ],
            "start_state_max_bounds_error": 0.1,
        },
        "plan_request_params": request_params(),
        "ompl_rrtc": {
            "plan_request_params": request_params(),
        },
    }


def _joint_state_seed(request: Any) -> tuple[list[str], list[float]]:
    urdf_path = Path(str(request.context.get("urdfPath") or ""))
    joint_types = _joint_types_for_request(request)
    joint_values = _native_start_joint_values(request, joint_types)
    joints = _active_urdf_joints(urdf_path)
    if not joints:
        joints = [(name, "revolute") for name in _joint_names(request)]
    names: list[str] = []
    positions: list[float] = []
    for name, joint_type in joints:
        names.append(name)
        del joint_type
        positions.append(float(joint_values.get(name, 0.0)))
    return names, positions


class _JointStateSeeder:
    def __init__(self, request: Any, *, duration_sec: float = 8.0, rate_hz: float = 30.0) -> None:
        self._request = request
        self._duration_sec = duration_sec
        self._rate_hz = rate_hz
        self._process: subprocess.Popen[bytes] | None = None

    def start(self) -> None:
        names, positions = _joint_state_seed(self._request)
        if not names:
            return
        self._process = subprocess.Popen(
            [
                sys.executable,
                "-m",
                "moveit2_server.joint_state_seed",
                "--names",
                json.dumps(names),
                "--positions",
                json.dumps(positions),
                "--duration",
                str(self._duration_sec),
                "--rate",
                str(self._rate_hz),
            ],
            stdout=subprocess.DEVNULL,
        )

    def stop(self) -> None:
        if not self._process or self._process.poll() is not None:
            return
        self._process.terminate()
        try:
            self._process.wait(timeout=1.0)
        except subprocess.TimeoutExpired:
            self._process.kill()
            self._process.wait(timeout=1.0)


class MoveItPyAdapter:
    """MoveIt 2 adapter used by the local ROS 2 MoveIt2 server."""

    def __init__(self) -> None:
        self._moveit_by_key: dict[str, Any] = {}

    def _config_dict(self, request: Any) -> dict[str, Any]:
        urdf_path = Path(str(request.context.get("urdfPath") or ""))
        srdf_path = Path(str(request.context.get("srdfPath") or ""))
        if not urdf_path.is_file():
            raise RuntimeError(f"URDF file does not exist: {urdf_path}")
        config: dict[str, Any] = {
            "robot_description": _robot_description_for_moveit(urdf_path),
            "robot_description_kinematics": _kinematics_config(request),
        }
        if srdf_path.is_file():
            config["robot_description_semantic"] = srdf_path.read_text(encoding="utf-8")
        config.update(_planning_pipeline_config(request))
        config["planning_scene_monitor_options"] = {
            "name": "planning_scene_monitor",
            "robot_description": "robot_description",
            "joint_state_topic": "/joint_states",
            "attached_collision_object_topic": "/moveit_cpp/planning_scene_monitor",
            "publish_planning_scene_topic": "/moveit_cpp/publish_planning_scene",
            "monitored_planning_scene_topic": "/moveit_cpp/monitored_planning_scene",
            "wait_for_initial_state_timeout": 5.0,
        }
        ompl_config = config.setdefault("ompl", {})
        if isinstance(ompl_config, dict):
            ompl_config.setdefault("planning_plugin", "ompl_interface/OMPLPlanner")
            ompl_config.setdefault("planning_plugins", ["ompl_interface/OMPLPlanner"])
            ompl_config.setdefault("request_adapters", [
                "default_planning_request_adapters/ResolveConstraintFrames",
                "default_planning_request_adapters/ValidateWorkspaceBounds",
                "default_planning_request_adapters/CheckStartStateBounds",
                "default_planning_request_adapters/CheckStartStateCollision",
            ])
            ompl_config.setdefault("start_state_max_bounds_error", 0.1)
        planning_scene_monitor_options = config.setdefault("planning_scene_monitor_options", {})
        if isinstance(planning_scene_monitor_options, dict):
            current_timeout = float(planning_scene_monitor_options.get("wait_for_initial_state_timeout") or 0.0)
            planning_scene_monitor_options["wait_for_initial_state_timeout"] = max(current_timeout, 5.0)
        return config

    def _moveit(self, request: Any) -> Any:
        try:
            import rclpy
            from moveit.planning import MoveItPy
        except ImportError as exc:
            raise RuntimeError(
                "moveit_py adapter requires a ROS 2 environment with rclpy and moveit_py. "
                f"Current interpreter: {sys.executable}. Start moveit2_server from a RoboStack/ROS shell."
            ) from exc

        cache_key = str(request.context.get("modelAssetHash") or request.context.get("urdfPath") or "")
        if cache_key in self._moveit_by_key:
            return self._moveit_by_key[cache_key]

        if not rclpy.ok():
            rclpy.init(args=None)
        node_hash = hashlib.sha1(cache_key.encode("utf-8")).hexdigest()[:10] if cache_key else "default"
        config = self._config_dict(request)
        joint_state_seeder = _JointStateSeeder(request)
        joint_state_seeder.start()
        try:
            try:
                moveit = MoveItPy(
                    node_name=f"moveit2_server_{node_hash}",
                    config_dict=config,
                )
            except TypeError:
                # Older bindings do not expose config_dict; they require equivalent
                # ROS parameters to be supplied by the launch environment.
                moveit = MoveItPy(node_name=f"moveit2_server_{node_hash}")
        finally:
            joint_state_seeder.stop()
        self._moveit_by_key[cache_key] = moveit
        return moveit

    def _planning_component(self, request: Any, planning_group: str) -> Any:
        return self._moveit(request).get_planning_component(planning_group)

    def _robot_state(self, request: Any) -> Any:
        try:
            from moveit.core.robot_state import RobotState
        except ImportError as exc:
            raise RuntimeError(
                f"moveit_py adapter could not import RobotState for {sys.executable}. "
                "Start moveit2_server from a RoboStack/ROS shell."
            ) from exc

        robot_state = RobotState(self._moveit(request).get_robot_model())
        joint_values = _native_start_joint_values(request)
        joint_names = _joint_names(request)
        positions = [float(joint_values.get(name, 0.0)) for name in joint_names]
        planning_group = _planning_group(request)
        if hasattr(robot_state, "set_joint_group_positions"):
            robot_state.set_joint_group_positions(planning_group, positions)
        elif hasattr(robot_state, "set_joint_positions"):
            robot_state.set_joint_positions(joint_names, positions)
        else:
            raise RuntimeError("moveit_py RobotState does not expose a supported joint setter")
        return robot_state

    def _pose_stamped(self, request: Any) -> tuple[Any, dict[str, Any]]:
        try:
            from geometry_msgs.msg import PoseStamped
        except ImportError as exc:
            raise RuntimeError(
                f"moveit_py adapter could not import geometry_msgs.msg.PoseStamped for {sys.executable}. "
                "Start moveit2_server from a RoboStack/ROS shell."
            ) from exc

        target = normalize_motion_target(request.payload)
        end_effector = _end_effector(request, target["endEffector"])
        pose = PoseStamped()
        pose.header.frame_id = target["frame"]
        pose.pose.position.x = target["xyz"][0]
        pose.pose.position.y = target["xyz"][1]
        pose.pose.position.z = target["xyz"][2]
        quat_xyzw = target.get("quat_xyzw") or (0.0, 0.0, 0.0, 1.0)
        pose.pose.orientation.x = quat_xyzw[0]
        pose.pose.orientation.y = quat_xyzw[1]
        pose.pose.orientation.z = quat_xyzw[2]
        pose.pose.orientation.w = quat_xyzw[3]
        return pose, end_effector

    def _position_only_ik(self, request: Any) -> bool:
        ik = request.command.get("ik", {}) if isinstance(request.command, dict) else {}
        return bool(ik.get("positionOnly", True))

    def _position_goal_constraints(self, target_pose: Any, end_effector: dict[str, Any]) -> list[Any]:
        try:
            from geometry_msgs.msg import Pose
            from moveit_msgs.msg import Constraints, PositionConstraint
            from shape_msgs.msg import SolidPrimitive
        except ImportError as exc:
            raise RuntimeError(
                f"moveit_py adapter could not import MoveIt position constraint messages for {sys.executable}. "
                "Start moveit2_server from a RoboStack/ROS shell."
            ) from exc

        sphere = SolidPrimitive()
        sphere.type = SolidPrimitive.SPHERE
        sphere.dimensions = [float(end_effector.get("positionTolerance") or 0.002)]

        center = Pose()
        center.orientation.w = 1.0
        center.position.x = target_pose.pose.position.x
        center.position.y = target_pose.pose.position.y
        center.position.z = target_pose.pose.position.z

        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = target_pose.header.frame_id
        position_constraint.link_name = str(end_effector.get("link") or "")
        position_constraint.constraint_region.primitives.append(sphere)
        position_constraint.constraint_region.primitive_poses.append(center)
        position_constraint.weight = 1.0

        constraints = Constraints()
        constraints.name = f"{position_constraint.link_name}_position_goal"
        constraints.position_constraints.append(position_constraint)
        return [constraints]

    def _joint_values_from_state(self, robot_state: Any, request: Any) -> dict[str, float]:
        joint_names = _joint_names(request)
        planning_group = _planning_group(request)
        if hasattr(robot_state, "get_joint_group_positions"):
            values = robot_state.get_joint_group_positions(planning_group)
        elif hasattr(robot_state, "get_joint_positions"):
            values = robot_state.get_joint_positions(joint_names)
        else:
            raise RuntimeError("moveit_py RobotState does not expose a supported joint getter")
        return {
            joint_name: float(values[index])
            for index, joint_name in enumerate(joint_names)
            if index < len(values)
        }

    def solve_pose(self, request: Any) -> dict[str, Any]:
        robot_state = self._robot_state(request)
        target_pose, end_effector = self._pose_stamped(request)
        planning_group = _planning_group(request)

        if not hasattr(robot_state, "set_from_ik"):
            raise RuntimeError("moveit_py RobotState does not expose set_from_ik")
        ik = request.command.get("ik", {}) if isinstance(request.command, dict) else {}
        timeout = float(ik.get("timeout") or 0.05)
        attempts = int(ik.get("attempts") or 1)
        try:
            ok = robot_state.set_from_ik(planning_group, target_pose.pose, end_effector.get("link"), timeout, attempts)
        except TypeError:
            try:
                ok = robot_state.set_from_ik(planning_group, target_pose.pose, end_effector.get("link"), timeout)
            except TypeError:
                ok = robot_state.set_from_ik(planning_group, target_pose.pose, end_effector.get("link"))
        if not ok:
            return {
                "ok": False,
                "message": "MoveIt did not find a pose solution",
            }
        joint_values = self._joint_values_from_state(robot_state, request)
        joint_types = _joint_types_for_request(request)
        return {
            "jointValuesByName": joint_values,
            "jointValuesByNameDeg": _native_to_legacy_joint_values(joint_values, joint_types),
            "positionOnly": self._position_only_ik(request),
            "residual": {"position": 0.0},
        }

    def _serialize_trajectory(self, trajectory: Any, joint_names: list[str], request: Any) -> dict[str, Any]:
        if hasattr(trajectory, "get_robot_trajectory_msg"):
            trajectory = trajectory.get_robot_trajectory_msg()
        joint_trajectory = getattr(trajectory, "joint_trajectory", trajectory)
        source_joint_names = list(getattr(joint_trajectory, "joint_names", []) or joint_names)
        joint_types = _joint_types_for_request(request)
        points = []
        for point in getattr(joint_trajectory, "points", []) or []:
            duration = getattr(point, "time_from_start", None)
            seconds = float(getattr(duration, "sec", 0)) + (float(getattr(duration, "nanosec", 0)) / 1_000_000_000)
            positions_by_name = {
                name: float(point.positions[index])
                for index, name in enumerate(source_joint_names)
                if index < len(getattr(point, "positions", []))
            }
            native_positions = [positions_by_name.get(name, 0.0) for name in joint_names]
            legacy_positions = [
                _native_to_legacy_joint_value(positions_by_name.get(name, 0.0), str(joint_types.get(name) or "revolute"))
                for name in joint_names
            ]
            points.append({
                "timeFromStartSec": seconds,
                "positions": native_positions,
                "positionsDeg": legacy_positions,
            })
        if len(points) > 1 and all(point["timeFromStartSec"] <= 0.0 for point in points):
            total_joint_delta = 0.0
            for previous, current in zip(points, points[1:], strict=False):
                total_joint_delta += sum(
                    abs(current["positionsDeg"][index] - previous["positionsDeg"][index])
                    for index in range(min(len(previous["positionsDeg"]), len(current["positionsDeg"])))
                )
            total_duration = min(max(total_joint_delta / 240.0, 0.75), 4.0)
            step_duration = total_duration / float(len(points) - 1)
            for index, point in enumerate(points):
                point["timeFromStartSec"] = index * step_duration
        return {
            "jointNames": joint_names,
            "points": points,
        }

    def plan_to_pose(self, request: Any) -> dict[str, Any]:
        target_pose, end_effector = self._pose_stamped(request)
        planning_group = _planning_group(request)
        planning_component = self._planning_component(request, planning_group)
        start_state = self._robot_state(request)
        planning_component.set_start_state(robot_state=start_state)
        if self._position_only_ik(request):
            try:
                planning_component.set_goal_state(
                    motion_plan_constraints=self._position_goal_constraints(target_pose, end_effector),
                )
            except TypeError:
                planning_component.set_goal_state(
                    pose_stamped_msg=target_pose,
                    pose_link=end_effector.get("link"),
                )
        else:
            planning_component.set_goal_state(
                pose_stamped_msg=target_pose,
                pose_link=end_effector.get("link"),
            )

        plan_result = planning_component.plan()
        if not plan_result:
            return {
                "ok": False,
                "message": "MoveIt planning did not find a trajectory",
            }
        joint_names = _joint_names(request)
        trajectory = self._serialize_trajectory(plan_result.trajectory, joint_names, request)
        final_joint_values: dict[str, float] = {}
        final_joint_values_legacy: dict[str, float] = {}
        if trajectory["points"]:
            final_joint_values = dict(zip(
                trajectory["jointNames"],
                trajectory["points"][-1]["positions"],
                strict=False,
            ))
            final_joint_values_legacy = dict(zip(
                trajectory["jointNames"],
                trajectory["points"][-1]["positionsDeg"],
                strict=False,
            ))
        return {
            "jointValuesByName": final_joint_values,
            "jointValuesByNameDeg": final_joint_values_legacy,
            "trajectory": trajectory,
            "positionOnly": self._position_only_ik(request),
            "residual": {"position": 0.0},
        }