first commit

text-to-cad-main/viewer/moveit2_server/__init__.py

@@ -0,0 +1,3 @@
+from pathlib import Path
+
+__path__.append(str(Path(__file__).resolve().parent / "moveit2_server"))

text-to-cad-main/viewer/moveit2_server/check-moveit2-server.sh

@@ -0,0 +1,14 @@
+#!/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}"

text-to-cad-main/viewer/moveit2_server/conda-common.sh

@@ -0,0 +1,53 @@
+#!/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
+}

text-to-cad-main/viewer/moveit2_server/environment.yml

@@ -0,0 +1,14 @@
+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

text-to-cad-main/viewer/moveit2_server/moveit2_server/__init__.py

@@ -0,0 +1 @@
+"""MoveIt2 websocket server for SRDF workflows."""

text-to-cad-main/viewer/moveit2_server/moveit2_server/context.py

@@ -0,0 +1,904 @@
+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,
+    }

text-to-cad-main/viewer/moveit2_server/moveit2_server/dispatcher.py

@@ -0,0 +1,29 @@
+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

text-to-cad-main/viewer/moveit2_server/moveit2_server/joint_state_seed.py

@@ -0,0 +1,61 @@
+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())

text-to-cad-main/viewer/moveit2_server/moveit2_server/moveit_py.py

@@ -0,0 +1,589 @@
+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},
+        }

text-to-cad-main/viewer/moveit2_server/moveit2_server/protocol.py

@@ -0,0 +1,222 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+import re
+from typing import Any
+
+
+SUPPORTED_REQUEST_TYPES = {"srdf.solvePose", "srdf.planToPose"}
+SUPPORTED_PROTOCOL_VERSION = 1
+_ABSOLUTE_PATH_RE = re.compile(r"(?<![\w.])/(?:[^\s'\"<>:,;(){}[\]]+/)+[^\s'\"<>:,;(){}[\]]+")
+
+
+class MotionProtocolError(ValueError):
+    """Raised when a MoveIt2 server request is malformed."""
+
+
+def _plain_object(value: Any) -> bool:
+    return isinstance(value, dict)
+
+
+def _string(value: Any, label: str) -> str:
+    normalized = str(value or "").strip()
+    if not normalized:
+        raise MotionProtocolError(f"{label} is required")
+    return normalized
+
+
+def _number(value: Any, label: str) -> float:
+    try:
+        numeric = float(value)
+    except (TypeError, ValueError) as exc:
+        raise MotionProtocolError(f"{label} must be a finite number") from exc
+    if numeric != numeric or numeric in (float("inf"), float("-inf")):
+        raise MotionProtocolError(f"{label} must be a finite number")
+    return numeric
+
+
+def normalize_xyz(value: Any, label: str = "target.xyz") -> tuple[float, float, float]:
+    if not isinstance(value, list) or len(value) != 3:
+        raise MotionProtocolError(f"{label} must be a 3-number array")
+    return (
+        _number(value[0], f"{label}[0]"),
+        _number(value[1], f"{label}[1]"),
+        _number(value[2], f"{label}[2]"),
+    )
+
+
+def normalize_quat_xyzw(value: Any, label: str = "target.quat_xyzw") -> tuple[float, float, float, float]:
+    if not isinstance(value, list) or len(value) != 4:
+        raise MotionProtocolError(f"{label} must be a 4-number array")
+    quat = (
+        _number(value[0], f"{label}[0]"),
+        _number(value[1], f"{label}[1]"),
+        _number(value[2], f"{label}[2]"),
+        _number(value[3], f"{label}[3]"),
+    )
+    norm = math.sqrt(sum(component * component for component in quat))
+    if norm <= 1e-12:
+        raise MotionProtocolError(f"{label} must not be zero length")
+    return tuple(component / norm for component in quat)
+
+
+def normalize_rpy(value: Any, label: str = "target.rpy") -> tuple[float, float, float]:
+    if not isinstance(value, list) or len(value) != 3:
+        raise MotionProtocolError(f"{label} must be a 3-number array")
+    return (
+        _number(value[0], f"{label}[0]"),
+        _number(value[1], f"{label}[1]"),
+        _number(value[2], f"{label}[2]"),
+    )
+
+
+def _quat_xyzw_from_rpy(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+    return normalize_quat_xyzw([
+        (sr * cp * cy) - (cr * sp * sy),
+        (cr * sp * cy) + (sr * cp * sy),
+        (cr * cp * sy) - (sr * sp * cy),
+        (cr * cp * cy) + (sr * sp * sy),
+    ])
+
+
+def normalize_joint_values(value: Any, label: str = "startJointValuesByNameDeg") -> dict[str, float]:
+    if value is None:
+        return {}
+    if not _plain_object(value):
+        raise MotionProtocolError(f"{label} must be an object")
+    normalized: dict[str, float] = {}
+    for raw_name, raw_value in value.items():
+        name = str(raw_name or "").strip()
+        if not name:
+            raise MotionProtocolError(f"{label} cannot include empty joint names")
+        normalized[name] = _number(raw_value, f"{label}.{name}")
+    return normalized
+
+
+def normalize_protocol_version(value: Any) -> int:
+    if value is None:
+        return SUPPORTED_PROTOCOL_VERSION
+    try:
+        version = int(value)
+    except (TypeError, ValueError) as exc:
+        raise MotionProtocolError("protocolVersion must be an integer") from exc
+    if version != SUPPORTED_PROTOCOL_VERSION:
+        raise MotionProtocolError(f"Unsupported protocolVersion {version}")
+    return version
+
+
+@dataclass(frozen=True)
+class WireMessage:
+    id: str
+    type: str
+    protocol_version: int
+    payload: dict[str, Any]
+
+
+@dataclass(frozen=True)
+class MotionRequest:
+    id: str
+    type: str
+    protocol_version: int
+    payload: dict[str, Any]
+    context: dict[str, Any]
+    command: dict[str, Any]
+
+
+def normalize_wire_message(message: Any) -> WireMessage:
+    if not _plain_object(message):
+        raise MotionProtocolError("MoveIt2 request must be an object")
+    request_id = _string(message.get("id"), "id")
+    request_type = _string(message.get("type"), "type")
+    protocol_version = normalize_protocol_version(message.get("protocolVersion"))
+    if request_type not in SUPPORTED_REQUEST_TYPES:
+        raise MotionProtocolError(f"Unsupported request type {request_type}")
+    payload = message.get("payload")
+    if not _plain_object(payload):
+        raise MotionProtocolError("payload must be an object")
+    return WireMessage(
+        id=request_id,
+        type=request_type,
+        protocol_version=protocol_version,
+        payload=payload,
+    )
+
+
+def normalize_request(message: Any, *, context: dict[str, Any]) -> MotionRequest:
+    wire = normalize_wire_message(message)
+    if not _plain_object(context):
+        raise MotionProtocolError("request context must be an object")
+    command = context.get("command")
+    if not _plain_object(command):
+        raise MotionProtocolError("request context command must be an object")
+    return MotionRequest(
+        id=wire.id,
+        type=wire.type,
+        protocol_version=wire.protocol_version,
+        payload=wire.payload,
+        context=context,
+        command=command,
+    )
+
+
+def normalize_motion_target(payload: dict[str, Any]) -> dict[str, Any]:
+    target = payload.get("target")
+    if not _plain_object(target):
+        raise MotionProtocolError("target must be an object")
+    has_quat = "quat_xyzw" in target
+    has_rpy = "rpy" in target
+    if has_quat and has_rpy:
+        raise MotionProtocolError("target must include exactly one of quat_xyzw or rpy")
+    quat_xyzw: tuple[float, float, float, float] | None = None
+    orientation_mode = "position_only"
+    if has_quat:
+        quat_xyzw = normalize_quat_xyzw(target.get("quat_xyzw"))
+        orientation_mode = "quat_xyzw"
+    elif has_rpy:
+        quat_xyzw = _quat_xyzw_from_rpy(*normalize_rpy(target.get("rpy")))
+        orientation_mode = "rpy"
+    return {
+        "endEffector": _string(target.get("endEffector"), "target.endEffector"),
+        "frame": _string(target.get("frame"), "target.frame"),
+        "xyz": normalize_xyz(target.get("xyz")),
+        "quat_xyzw": quat_xyzw,
+        "orientationMode": orientation_mode,
+    }
+
+
+def success_response(request_id: str, result: dict[str, Any]) -> dict[str, Any]:
+    return {
+        "id": request_id,
+        "ok": True,
+        "result": result,
+    }
+
+
+def _sanitize_error_message(message: str) -> str:
+    def replace(match: re.Match[str]) -> str:
+        path = match.group(0)
+        basename = path.rstrip("/").rsplit("/", 1)[-1]
+        return f".../{basename}" if basename else "<path>"
+
+    return _ABSOLUTE_PATH_RE.sub(replace, message)
+
+
+def error_response(request_id: str, error: BaseException, *, debug: bool = False) -> dict[str, Any]:
+    message = str(error)
+    if not debug:
+        message = _sanitize_error_message(message)
+    return {
+        "id": request_id,
+        "ok": False,
+        "error": {
+            "code": error.__class__.__name__,
+            "message": message,
+        },
+    }

text-to-cad-main/viewer/moveit2_server/moveit2_server/server.py

@@ -0,0 +1,108 @@
+from __future__ import annotations
+
+import argparse
+import asyncio
+import importlib
+import json
+import os
+from pathlib import Path
+from typing import Any
+
+from moveit2_server.context import DEFAULT_CAD_DIRECTORY, build_moveit2_context
+from moveit2_server.dispatcher import dispatch
+from moveit2_server.protocol import error_response, normalize_request, normalize_wire_message, success_response
+
+
+def _json_dumps(message: dict[str, Any]) -> str:
+    return json.dumps(message, separators=(",", ":"))
+
+
+async def handle_message(raw_message: str, *, repo_root: Path) -> str:
+    request_id = ""
+    debug_errors = os.environ.get("MOVEIT2_SERVER_DEBUG_ERRORS", "").strip().lower() in {"1", "true", "yes", "on"}
+    try:
+        message = json.loads(raw_message)
+        wire = normalize_wire_message(message)
+        request_id = wire.id
+        payload_dir = wire.payload.get("dir", DEFAULT_CAD_DIRECTORY)
+        context = build_moveit2_context(
+            repo_root=repo_root,
+            dir=payload_dir,
+            file=wire.payload.get("file"),
+            type=wire.type,
+            payload=wire.payload,
+        )
+        request = normalize_request(message, context=context)
+        return _json_dumps(success_response(request.id, dispatch(request)))
+    except Exception as exc:
+        return _json_dumps(error_response(request_id, exc, debug=debug_errors))
+
+
+async def handle_client(websocket: Any, path: str | None = None, *, repo_root: Path) -> None:
+    async for raw_message in websocket:
+        await websocket.send(await handle_message(str(raw_message), repo_root=repo_root))
+
+
+async def run_server(*, repo_root: Path, host: str, port: int) -> None:
+    try:
+        from websockets.asyncio.server import serve
+    except ImportError:
+        from websockets import serve
+
+    async with serve(lambda websocket, path=None: handle_client(websocket, path, repo_root=repo_root), host, port):
+        print(f"moveit2_server listening on ws://{host}:{port}/ws", flush=True)
+        await asyncio.Future()
+
+
+def _check_import(module_name: str, attribute_path: str | None = None) -> str:
+    module = importlib.import_module(module_name)
+    current: Any = module
+    if attribute_path:
+        for attribute in attribute_path.split("."):
+            current = getattr(current, attribute)
+    return str(getattr(module, "__file__", "built-in"))
+
+
+def check_environment(*, repo_root: Path) -> dict[str, Any]:
+    checks = {
+        "repoRoot": str(repo_root),
+        "rmwImplementation": os.environ.get("RMW_IMPLEMENTATION", ""),
+        "imports": {
+            "websockets": _check_import("websockets"),
+            "rclpy": _check_import("rclpy"),
+            "geometry_msgs.msg.PoseStamped": _check_import("geometry_msgs.msg", "PoseStamped"),
+            "moveit.planning.MoveItPy": _check_import("moveit.planning", "MoveItPy"),
+        },
+    }
+    return checks
+
+
+def build_arg_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(prog="moveit2_server")
+    parser.add_argument(
+        "--repo-root",
+        default=str(Path.cwd()),
+        help="Repository root that contains the CAD scan directory.",
+    )
+    parser.add_argument("--host", default="127.0.0.1")
+    parser.add_argument("--port", type=int, default=8765)
+    parser.add_argument(
+        "--check",
+        action="store_true",
+        help="Verify required Python, ROS 2, MoveIt, and websocket imports, then exit.",
+    )
+    return parser
+
+
+def main(argv: list[str] | None = None) -> int:
+    args = build_arg_parser().parse_args(argv)
+    repo_root = Path(args.repo_root).resolve()
+    if args.check:
+        print(json.dumps(check_environment(repo_root=repo_root), indent=2, sort_keys=True))
+        return 0
+    asyncio.run(run_server(repo_root=repo_root, host=args.host, port=args.port))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

text-to-cad-main/viewer/moveit2_server/moveit2_server/srdf_source.py

@@ -0,0 +1,261 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from math import isfinite
+from pathlib import Path
+from typing import Literal
+import xml.etree.ElementTree as ET
+
+
+SRDF_SUFFIX = ".srdf"
+SRDF_METADATA_NAMESPACE = "https://text-to-cad.dev/srdf"
+LEGACY_EXPLORER_NAMESPACE = "https://text-to-cad.dev/explorer"
+
+
+class SrdfSourceError(ValueError):
+    pass
+
+
+@dataclass(frozen=True)
+class SrdfChain:
+    base_link: str
+    tip_link: str
+
+
+@dataclass(frozen=True)
+class SrdfPlanningGroup:
+    name: str
+    joint_names: tuple[str, ...]
+    link_names: tuple[str, ...]
+    chains: tuple[SrdfChain, ...]
+    subgroups: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class SrdfEndEffector:
+    name: str
+    parent_link: str
+    group: str
+    parent_group: str
+
+
+@dataclass(frozen=True)
+class SrdfGroupState:
+    name: str
+    group: str
+    joint_values_by_name: dict[str, float]
+
+    @property
+    def joint_values_by_name_rad(self) -> dict[str, float]:
+        return self.joint_values_by_name
+
+
+@dataclass(frozen=True)
+class SrdfDisabledCollisionPair:
+    link1: str
+    link2: str
+    reason: str
+    source: Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"] = "manual"
+
+
+@dataclass(frozen=True)
+class SrdfSource:
+    file_ref: str
+    source_path: Path
+    robot_name: str
+    urdf_ref: str
+    planning_groups: tuple[SrdfPlanningGroup, ...]
+    end_effectors: tuple[SrdfEndEffector, ...]
+    group_states: tuple[SrdfGroupState, ...]
+    disabled_collision_pairs: tuple[SrdfDisabledCollisionPair, ...]
+
+
+def read_srdf_source(srdf_path: Path) -> SrdfSource:
+    resolved_path = srdf_path.resolve()
+    if resolved_path.suffix.lower() != SRDF_SUFFIX:
+        raise SrdfSourceError(f"{resolved_path} is not an SRDF source file")
+    try:
+        root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
+    except (OSError, ET.ParseError) as exc:
+        raise SrdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as SRDF XML") from exc
+    return parse_srdf_root(root, source_path=resolved_path)
+
+
+def parse_srdf_root(root: ET.Element, *, source_path: Path) -> SrdfSource:
+    if root.tag != "robot":
+        raise SrdfSourceError(f"{_relative_to_repo(source_path)} root element must be <robot>")
+    robot_name = str(root.attrib.get("name") or "").strip()
+    if not robot_name:
+        raise SrdfSourceError(f"{_relative_to_repo(source_path)} robot name is required")
+
+    planning_groups: list[SrdfPlanningGroup] = []
+    group_names: list[str] = []
+    for group_element in root.findall("group"):
+        name = str(group_element.attrib.get("name") or "").strip()
+        if not name:
+            raise SrdfSourceError(f"{_relative_to_repo(source_path)} group name is required")
+        group_names.append(name)
+        planning_groups.append(
+            SrdfPlanningGroup(
+                name=name,
+                joint_names=_child_names(group_element, "joint"),
+                link_names=_child_names(group_element, "link"),
+                chains=tuple(
+                    SrdfChain(
+                        base_link=str(chain.attrib.get("base_link") or "").strip(),
+                        tip_link=str(chain.attrib.get("tip_link") or "").strip(),
+                    )
+                    for chain in group_element.findall("chain")
+                ),
+                subgroups=_child_names(group_element, "group"),
+            )
+        )
+    _raise_on_duplicates(group_names, source_path=source_path, label="group")
+
+    end_effectors: list[SrdfEndEffector] = []
+    end_effector_names: list[str] = []
+    for element in root.findall("end_effector"):
+        name = str(element.attrib.get("name") or "").strip()
+        if not name:
+            raise SrdfSourceError(f"{_relative_to_repo(source_path)} end_effector name is required")
+        end_effector_names.append(name)
+        end_effectors.append(
+            SrdfEndEffector(
+                name=name,
+                parent_link=str(element.attrib.get("parent_link") or "").strip(),
+                group=str(element.attrib.get("group") or "").strip(),
+                parent_group=str(element.attrib.get("parent_group") or "").strip(),
+            )
+        )
+    _raise_on_duplicates(end_effector_names, source_path=source_path, label="end_effector")
+
+    group_states: list[SrdfGroupState] = []
+    group_state_keys: list[str] = []
+    for element in root.findall("group_state"):
+        name = str(element.attrib.get("name") or "").strip()
+        group = str(element.attrib.get("group") or "").strip()
+        if not name or not group:
+            raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state requires name and group")
+        group_state_keys.append(f"{group}/{name}")
+        joint_values: dict[str, float] = {}
+        joint_names: list[str] = []
+        for joint_element in element.findall("joint"):
+            joint_name = str(joint_element.attrib.get("name") or "").strip()
+            if not joint_name:
+                raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state joint name is required")
+            joint_names.append(joint_name)
+            try:
+                joint_value = float(joint_element.attrib.get("value"))
+            except (TypeError, ValueError) as exc:
+                raise SrdfSourceError(
+                    f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be numeric"
+                ) from exc
+            if not isfinite(joint_value):
+                raise SrdfSourceError(
+                    f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be finite"
+                )
+            joint_values[joint_name] = joint_value
+        _raise_on_duplicates(joint_names, source_path=source_path, label=f"group_state {name!r} joint")
+        group_states.append(SrdfGroupState(name=name, group=group, joint_values_by_name=joint_values))
+    _raise_on_duplicates(group_state_keys, source_path=source_path, label="group_state")
+
+    disabled_pairs: list[SrdfDisabledCollisionPair] = []
+    pair_keys: list[str] = []
+    for element in root.findall("disable_collisions"):
+        link1 = str(element.attrib.get("link1") or "").strip()
+        link2 = str(element.attrib.get("link2") or "").strip()
+        if not link1 or not link2:
+            raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions requires link1 and link2")
+        if link1 == link2:
+            raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions cannot repeat the same link")
+        reason = str(element.attrib.get("reason") or "").strip()
+        if not reason:
+            raise SrdfSourceError(
+                f"{_relative_to_repo(source_path)} disable_collisions for {link1!r}/{link2!r} requires a reason"
+            )
+        pair_key = "/".join(sorted((link1, link2)))
+        pair_keys.append(pair_key)
+        disabled_pairs.append(
+            SrdfDisabledCollisionPair(
+                link1=link1,
+                link2=link2,
+                reason=reason,
+                source=_disabled_collision_source(reason),
+            )
+        )
+    _raise_on_duplicates(pair_keys, source_path=source_path, label="disable_collisions")
+
+    return SrdfSource(
+        file_ref=_relative_to_repo(source_path.resolve()),
+        source_path=source_path.resolve(),
+        robot_name=robot_name,
+        urdf_ref=_linked_urdf_ref(root),
+        planning_groups=tuple(planning_groups),
+        end_effectors=tuple(end_effectors),
+        group_states=tuple(group_states),
+        disabled_collision_pairs=tuple(disabled_pairs),
+    )
+
+
+def _local_name(tag: str) -> str:
+    return str(tag or "").rsplit("}", 1)[-1].split(":", 1)[-1]
+
+
+def _linked_urdf_ref(root: ET.Element) -> str:
+    for child in list(root):
+        if _local_name(child.tag) != "urdf":
+            continue
+        tag = str(child.tag or "")
+        if (
+            tag.startswith(f"{{{SRDF_METADATA_NAMESPACE}}}")
+            or tag.startswith(f"{{{LEGACY_EXPLORER_NAMESPACE}}}")
+            or tag.startswith("tcad:")
+            or tag.startswith("explorer:")
+        ):
+            return str(child.attrib.get("path") or "").strip()
+    return ""
+
+
+def _child_names(element: ET.Element, tag: str) -> tuple[str, ...]:
+    names: list[str] = []
+    for child in element.findall(tag):
+        name = str(child.attrib.get("name") or "").strip()
+        if name:
+            names.append(name)
+    return tuple(names)
+
+
+def _raise_on_duplicates(names: list[str], *, source_path: Path, label: str) -> None:
+    seen = set()
+    duplicates = []
+    for name in names:
+        if name in seen:
+            duplicates.append(name)
+        seen.add(name)
+    if duplicates:
+        raise SrdfSourceError(
+            f"{_relative_to_repo(source_path)} has duplicate {label} name(s): {sorted(set(duplicates))!r}"
+        )
+
+
+def _disabled_collision_source(
+    reason: str,
+) -> Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"]:
+    normalized = str(reason or "").strip().lower()
+    if "adjacent" in normalized:
+        return "adjacent"
+    if any(token in normalized for token in ("never", "always", "sample", "default")):
+        return "sampled"
+    if "setup" in normalized or "assistant" in normalized:
+        return "setup_assistant"
+    if "assum" in normalized:
+        return "assumed"
+    return "manual"
+
+
+def _relative_to_repo(path: Path) -> str:
+    resolved = path.resolve()
+    try:
+        return resolved.relative_to(Path.cwd().resolve()).as_posix()
+    except ValueError:
+        return resolved.as_posix()

text-to-cad-main/viewer/moveit2_server/pyproject.toml

@@ -0,0 +1,21 @@
+[build-system]
+requires = ["setuptools>=69"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "moveit2_server"
+version = "0.3.9"
+description = "Local MoveIt2 websocket server for SRDF solve and planning requests."
+requires-python = ">=3.12"
+dependencies = [
+  "websockets>=12",
+]
+
+[project.scripts]
+moveit2_server = "moveit2_server.server:main"
+
+[tool.setuptools]
+packages = ["moveit2_server"]
+
+[tool.setuptools.package-dir]
+moveit2_server = "moveit2_server"

text-to-cad-main/viewer/moveit2_server/run-moveit2-server.sh

@@ -0,0 +1,19 @@
+#!/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}}"
+
+exec "${CONDA}" run --no-capture-output -n "${MOVEIT2_SERVER_CONDA_ENV_NAME}" \
+  python -m moveit2_server.server \
+  --repo-root "${REPO_ROOT}" \
+  --host "${MOVEIT2_SERVER_HOST:-127.0.0.1}" \
+  --port "${MOVEIT2_SERVER_PORT:-8765}" \
+  "$@"

text-to-cad-main/viewer/moveit2_server/setup.sh

@@ -0,0 +1,16 @@
+#!/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}" env update --file "${SCRIPT_DIR}/environment.yml" --prune
+"${CONDA}" run --no-capture-output -n "${MOVEIT2_SERVER_CONDA_ENV_NAME}" python -m pip install -e "${SCRIPT_DIR}"
+"${CONDA}" run --no-capture-output -n "${MOVEIT2_SERVER_CONDA_ENV_NAME}" python -m moveit2_server.server --check --repo-root "${REPO_ROOT}"