From 5d482821e4065f0939e4aa47beea0b944c19decb Mon Sep 17 00:00:00 2001 From: Mustafa Bhadsorawala <39084056+mustafab0@users.noreply.github.com> Date: Sun, 21 Jun 2026 22:28:38 -0700 Subject: [PATCH 1/8] refactored manipulation blueprints for better organization (#2553) --- dimos/control/blueprints/_hardware.py | 209 --------- dimos/control/blueprints/basic.py | 105 +---- dimos/control/blueprints/mobile.py | 9 +- dimos/control/blueprints/teleop.py | 264 ----------- dimos/manipulation/blueprints.py | 409 +----------------- dimos/robot/all_blueprints.py | 72 +-- dimos/robot/manipulators/_modeling.py | 59 +++ dimos/robot/manipulators/a750/blueprints.py | 119 ----- .../manipulators/a750/blueprints/teleop.py | 51 +++ dimos/robot/manipulators/a750/config.py | 123 ++++++ .../manipulators/common/agent_prompts.py | 109 +++++ dimos/robot/manipulators/common/blueprints.py | 128 ++++++ .../manipulators/common/mixed.py} | 75 ++-- dimos/robot/manipulators/common/mock.py | 62 +++ dimos/robot/manipulators/common/sim.py | 31 ++ dimos/robot/manipulators/common/topics.py | 30 ++ .../robot/manipulators/openarm/blueprints.py | 289 ------------- .../manipulators/openarm/blueprints/basic.py | 74 ++++ .../openarm/blueprints/planner.py | 60 +++ .../manipulators/openarm/blueprints/teleop.py | 71 +++ dimos/robot/manipulators/openarm/config.py | 128 ++++++ dimos/robot/manipulators/piper/blueprints.py | 108 ----- .../manipulators/piper/blueprints/basic.py | 39 ++ .../manipulators/piper/blueprints/teleop.py | 104 +++++ dimos/robot/manipulators/piper/config.py | 131 ++++++ dimos/robot/manipulators/xarm/blueprints.py | 161 ------- .../manipulators/xarm/blueprints/agentic.py | 46 ++ .../manipulators/xarm/blueprints/basic.py | 91 ++++ .../xarm/blueprints/perception.py | 61 +++ .../xarm/blueprints/simulation.py | 72 +++ .../manipulators/xarm/blueprints/teleop.py | 183 ++++++++ dimos/robot/manipulators/xarm/config.py | 208 +++++++++ dimos/teleop/quest/blueprints.py | 12 +- dimos/teleop/quest_hosted/blueprints.py | 2 +- docs/capabilities/manipulation/a750.md | 9 +- .../manipulation/adding_a_custom_arm.md | 2 +- .../manipulation/openarm_integration.md | 8 +- docs/capabilities/manipulation/readme.md | 11 +- 38 files changed, 1976 insertions(+), 1749 deletions(-) delete mode 100644 dimos/control/blueprints/_hardware.py delete mode 100644 dimos/control/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/_modeling.py delete mode 100644 dimos/robot/manipulators/a750/blueprints.py create mode 100644 dimos/robot/manipulators/a750/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/a750/config.py create mode 100644 dimos/robot/manipulators/common/agent_prompts.py create mode 100644 dimos/robot/manipulators/common/blueprints.py rename dimos/{control/blueprints/dual.py => robot/manipulators/common/mixed.py} (50%) create mode 100644 dimos/robot/manipulators/common/mock.py create mode 100644 dimos/robot/manipulators/common/sim.py create mode 100644 dimos/robot/manipulators/common/topics.py delete mode 100644 dimos/robot/manipulators/openarm/blueprints.py create mode 100644 dimos/robot/manipulators/openarm/blueprints/basic.py create mode 100644 dimos/robot/manipulators/openarm/blueprints/planner.py create mode 100644 dimos/robot/manipulators/openarm/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/openarm/config.py delete mode 100644 dimos/robot/manipulators/piper/blueprints.py create mode 100644 dimos/robot/manipulators/piper/blueprints/basic.py create mode 100644 dimos/robot/manipulators/piper/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/piper/config.py delete mode 100644 dimos/robot/manipulators/xarm/blueprints.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/agentic.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/basic.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/perception.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/simulation.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/xarm/config.py diff --git a/dimos/control/blueprints/_hardware.py b/dimos/control/blueprints/_hardware.py deleted file mode 100644 index f652265b35..0000000000 --- a/dimos/control/blueprints/_hardware.py +++ /dev/null @@ -1,209 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Hardware component helpers for coordinator blueprints.""" - -from __future__ import annotations - -from dimos.control.components import ( - HardwareComponent, - HardwareType, - make_joints, - make_twist_base_joints, -) -from dimos.core.global_config import global_config -from dimos.utils.data import LfsPath - -PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") -XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") -XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") -A750_FK_MODEL = LfsPath("a750_description/urdf/a750_rev1_no_gripper.urdf") - -XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") -XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") -PIPER_SIM_PATH = LfsPath("piper/scene.xml") - - -def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: - if home_joints is None: - return {} - return {"initial_positions": home_joints} - - -def manipulator( - hw_id: str, - dof: int, - *, - adapter_type: str = "mock", - address: str | None = None, - gripper: bool = False, - gripper_suffix: str = "gripper", - auto_enable: bool = True, - adapter_kwargs: dict[str, object] | None = None, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Create a manipulator hardware component with DimOS joint names.""" - kwargs = _adapter_kwargs(home_joints) - if adapter_kwargs: - kwargs.update(adapter_kwargs) - return HardwareComponent( - hardware_id=hw_id, - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints(hw_id, dof), - adapter_type=adapter_type, - address=address, - auto_enable=auto_enable, - gripper_joints=[f"{hw_id}/{gripper_suffix}"] if gripper else [], - adapter_kwargs=kwargs, - ) - - -def mock_arm( - hw_id: str = "arm", - dof: int = 7, - *, - gripper: bool = False, - gripper_suffix: str = "gripper", - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Mock manipulator with no real hardware.""" - return manipulator( - hw_id, - dof, - gripper=gripper, - gripper_suffix=gripper_suffix, - home_joints=home_joints, - ) - - -def xarm7( - hw_id: str = "arm", - *, - gripper: bool = False, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """xArm7 hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 7, - adapter_type="sim_mujoco", - address=str(XARM7_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.xarm7_ip - if mock_without_address and not address: - return mock_arm(hw_id, 7, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 7, - adapter_type="xarm", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def xarm6( - hw_id: str = "arm", - *, - gripper: bool = False, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """xArm6 hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 6, - adapter_type="sim_mujoco", - address=str(XARM6_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.xarm6_ip - if mock_without_address and not address: - return mock_arm(hw_id, 6, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 6, - adapter_type="xarm", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def piper( - hw_id: str = "arm", - *, - gripper: bool = True, - mock_without_address: bool = False, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Piper hardware, MuJoCo when --simulation is set, or mock if requested.""" - if global_config.simulation: - return manipulator( - hw_id, - 6, - adapter_type="sim_mujoco", - address=str(PIPER_SIM_PATH), - gripper=gripper, - home_joints=home_joints, - ) - address = global_config.can_port or "can0" - if mock_without_address and not global_config.can_port: - return mock_arm(hw_id, 6, gripper=gripper, home_joints=home_joints) - return manipulator( - hw_id, - 6, - adapter_type="piper", - address=address, - gripper=gripper, - home_joints=home_joints, - ) - - -def a750(hw_id: str = "arm", *, mock_without_address: bool = False) -> HardwareComponent: - """A-750 hardware or mock when no device path is configured.""" - home_joints = [0.0, 0.0, -1.5707963267948966, 0.0, 0.0, 0.0] - if mock_without_address and not global_config.device_path: - return mock_arm( - hw_id, - 6, - gripper=True, - gripper_suffix="finger", - home_joints=home_joints, - ) - return manipulator( - hw_id, - 6, - adapter_type="a750", - address=global_config.device_path or "/dev/ttyACM0", - gripper=True, - gripper_suffix="finger", - home_joints=home_joints, - ) - - -def mock_twist_base(hw_id: str = "base") -> HardwareComponent: - """Mock holonomic twist base (3-DOF: vx, vy, wz).""" - return HardwareComponent( - hardware_id=hw_id, - hardware_type=HardwareType.BASE, - joints=make_twist_base_joints(hw_id), - adapter_type="mock_twist_base", - ) diff --git a/dimos/control/blueprints/basic.py b/dimos/control/blueprints/basic.py index 214d01275e..40194bae95 100644 --- a/dimos/control/blueprints/basic.py +++ b/dimos/control/blueprints/basic.py @@ -12,117 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Single-arm coordinator blueprints with trajectory control. - -Each arm blueprint switches between real hardware and MuJoCo via `--simulation`. +"""Basic control coordinator blueprint. Usage: - dimos run coordinator-mock # Mock 7-DOF arm - dimos run coordinator-xarm7 # XArm7 real - dimos --simulation run coordinator-xarm7 # XArm7 in MuJoCo - dimos run coordinator-xarm6 # XArm6 real - dimos --simulation run coordinator-xarm6 # XArm6 in MuJoCo - dimos run coordinator-piper # Piper real (CAN) - dimos --simulation run coordinator-piper # Piper in MuJoCo + dimos run coordinator-basic """ from __future__ import annotations -from dimos.control.blueprints._hardware import ( - PIPER_SIM_PATH, - XARM6_SIM_PATH, - XARM7_SIM_PATH, - mock_arm, - piper, - xarm6, - xarm7, -) -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.global_config import global_config -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule - -_is_sim = global_config.simulation - - -def _mujoco_if_sim(sim_path: str, dof: int) -> tuple[Blueprint, ...]: - if not _is_sim: - return () - return (MujocoSimModule.blueprint(address=sim_path, headless=False, dof=dof),) +from dimos.control.coordinator import ControlCoordinator - -# Minimal blueprint (no hardware, no tasks) coordinator_basic = ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", ) - -# Mock 7-DOF arm (for testing) -_mock_hw = mock_arm("arm", 7) - -coordinator_mock = ControlCoordinator.blueprint( - hardware=[_mock_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_mock_hw.joints, - priority=10, - ) - ], -) - -# XArm7 (real, or MuJoCo with --simulation) -_xarm7_hw = xarm7("arm") - -coordinator_xarm7 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm7_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(XARM7_SIM_PATH), len(_xarm7_hw.joints)), -) - -# XArm6 (real, or MuJoCo with --simulation) -_xarm6_hw = xarm6("arm", gripper=True) - -coordinator_xarm6 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="traj_xarm", - type="trajectory", - joint_names=_xarm6_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(XARM6_SIM_PATH), len(_xarm6_hw.joints)), -) - -# Piper 6-DOF (CAN bus, or MuJoCo with --simulation) -_piper_hw = piper("arm") - -coordinator_piper = autoconnect( - ControlCoordinator.blueprint( - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="traj_piper", - type="trajectory", - joint_names=_piper_hw.joints, - priority=10, - ) - ], - ), - *_mujoco_if_sim(str(PIPER_SIM_PATH), len(_piper_hw.joints)), -) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 64a9090899..ab0230ee64 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -26,10 +26,10 @@ import os -from dimos.control.blueprints._hardware import mock_arm from dimos.control.components import ( HardwareComponent, HardwareType, + make_joints, make_twist_base_joints, ) from dimos.control.coordinator import ControlCoordinator, TaskConfig @@ -195,7 +195,12 @@ def _flowbase_twist_base( # Mock arm (7-DOF) + mock holonomic base (3-DOF) -_mock_arm_hw = mock_arm("arm", 7) +_mock_arm_hw = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", +) coordinator_mobile_manip_mock = ControlCoordinator.blueprint( hardware=[_mock_arm_hw, _mock_twist_base()], diff --git a/dimos/control/blueprints/teleop.py b/dimos/control/blueprints/teleop.py deleted file mode 100644 index 993cc15267..0000000000 --- a/dimos/control/blueprints/teleop.py +++ /dev/null @@ -1,264 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Advanced control coordinator blueprints: servo, velocity, cartesian IK, and teleop IK. - -Teleop blueprints switch between MuJoCo and real hardware via `--simulation`. - -Usage: - dimos run coordinator-teleop-xarm6 # Real XArm6 (TeleopIK) - dimos --simulation run coordinator-teleop-xarm6 # XArm6 in MuJoCo sim - dimos run coordinator-teleop-xarm7 # Real XArm7 - dimos --simulation run coordinator-teleop-xarm7 # XArm7 in MuJoCo sim - dimos run coordinator-teleop-piper # Real Piper - dimos --simulation run coordinator-teleop-piper # Piper in MuJoCo sim - dimos run coordinator-servo-xarm6 # Servo streaming (real-only) - dimos run coordinator-velocity-xarm6 # Velocity streaming (real-only) - dimos run coordinator-combined-xarm6 # Servo + velocity (real-only) - dimos run coordinator-cartesian-ik-mock # Cartesian IK (mock) - dimos run coordinator-cartesian-ik-piper # Cartesian IK (Piper, real-only) - dimos run coordinator-teleop-dual # TeleopIK dual arm (real-only) -""" - -from __future__ import annotations - -from dimos.control.blueprints._hardware import ( - PIPER_FK_MODEL, - PIPER_SIM_PATH, - XARM6_FK_MODEL, - XARM6_SIM_PATH, - XARM7_FK_MODEL, - XARM7_SIM_PATH, - manipulator, - mock_arm, - piper, - xarm6, - xarm7, -) -from dimos.control.components import make_gripper_joints -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.global_config import global_config -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule - -_is_sim = global_config.simulation - - -def _mujoco_if_sim(sim_path: str, dof: int) -> tuple[Blueprint, ...]: - if not _is_sim: - return () - return (MujocoSimModule.blueprint(address=sim_path, headless=False, dof=dof),) - - -_xarm6_hw = manipulator( - "arm", - 6, - adapter_type="xarm", - address=global_config.xarm6_ip, - gripper=True, -) -_piper_hw = manipulator( - "arm", - 6, - adapter_type="piper", - address=global_config.can_port or "can0", - gripper=True, -) - -_xarm7_teleop_hw = xarm7("arm", gripper=True) -_xarm6_teleop_hw = xarm6("arm", gripper=True) -_piper_teleop_hw = piper("arm") - -# XArm6 servo - streaming position control -coordinator_servo_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# XArm6 velocity control - streaming velocity for joystick -coordinator_velocity_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# XArm6 combined (servo + velocity tasks) -coordinator_combined_xarm6 = ControlCoordinator.blueprint( - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="servo_arm", - type="servo", - joint_names=_xarm6_hw.joints, - priority=10, - ), - TaskConfig( - name="velocity_arm", - type="velocity", - joint_names=_xarm6_hw.joints, - priority=10, - ), - ], -) - -# Mock 6-DOF arm with CartesianIK -_mock_6dof_hw = mock_arm("arm", 6) - -coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( - hardware=[_mock_6dof_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_mock_6dof_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], -) - -# Piper arm with CartesianIK -coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_piper_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], -) - -# XArm7 with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_xarm7 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm7_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm7_teleop_hw.joints, - priority=10, - params={ - "model_path": XARM7_FK_MODEL, - "ee_joint_id": 7, - "hand": "right", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.85, - "gripper_closed_pos": 0.0, - }, - ), - ], - ), - *_mujoco_if_sim(str(XARM7_SIM_PATH), len(_xarm7_teleop_hw.joints)), -) - -# XArm6 with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_xarm6 = autoconnect( - ControlCoordinator.blueprint( - hardware=[_xarm6_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm6_teleop_hw.joints, - priority=10, - params={ - "model_path": XARM6_FK_MODEL, - "ee_joint_id": 6, - "hand": "right", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.85, - "gripper_closed_pos": 0.0, - }, - ), - ], - ), - *_mujoco_if_sim(str(XARM6_SIM_PATH), len(_xarm6_teleop_hw.joints)), -) - -# Piper with TeleopIK (real, or MuJoCo with --simulation) -coordinator_teleop_piper = autoconnect( - ControlCoordinator.blueprint( - hardware=[_piper_teleop_hw], - tasks=[ - TaskConfig( - name="teleop_piper", - type="teleop_ik", - joint_names=_piper_teleop_hw.joints, - priority=10, - params={ - "model_path": PIPER_FK_MODEL, - "ee_joint_id": 6, - "hand": "left", - "gripper_joint": make_gripper_joints("arm")[0], - "gripper_open_pos": 0.0, - "gripper_closed_pos": 0.035, - }, - ), - ], - ), - *_mujoco_if_sim(str(PIPER_SIM_PATH), len(_piper_teleop_hw.joints)), -) - -# Dual arm teleop: XArm6 + Piper with TeleopIK (real-only) -_xarm6_dual_hw = manipulator( - "xarm_arm", - 6, - adapter_type="xarm", - address=global_config.xarm6_ip, - gripper=True, -) -_piper_dual_hw = manipulator( - "piper_arm", - 6, - adapter_type="piper", - address=global_config.can_port, - gripper=True, -) - -coordinator_teleop_dual = ControlCoordinator.blueprint( - hardware=[_xarm6_dual_hw, _piper_dual_hw], - tasks=[ - TaskConfig( - name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm6_dual_hw.joints, - priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"}, - ), - TaskConfig( - name="teleop_piper", - type="teleop_ik", - joint_names=_piper_dual_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"}, - ), - ], -) diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 5207135638..0dd9edbd9e 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -12,405 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -Manipulation blueprints. - -Quick start: - # 1. Verify manipulation deps load correctly (standalone, no hardware): - dimos run xarm6-planner-only +"""Compatibility exports for manipulation blueprints. - # 2. Keyboard teleop with mock arm: - dimos run keyboard-teleop-xarm7 - - # 3. Interactive RPC client (plan, preview, execute from Python): - dimos run xarm7-planner-coordinator - python -i -m dimos.manipulation.planning.examples.manipulation_client +Robot-owned manipulation blueprints now live under ``dimos.robot.manipulators``. """ -import math -from pathlib import Path -from typing import Any - -from dimos.agents.mcp.mcp_client import McpClient -from dimos.agents.mcp.mcp_server import McpServer -from dimos.control.blueprints._hardware import XARM7_SIM_PATH, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule -from dimos.utils.data import LfsPath -from dimos.visualization.rerun.bridge import RerunBridgeModule - -XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("right_inner_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "left_outer_knuckle"), - ("right_inner_knuckle", "right_finger"), - ("left_inner_knuckle", "left_finger"), - ("left_finger", "right_finger"), - ("left_outer_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "right_inner_knuckle"), - ("left_outer_knuckle", "right_finger"), - ("right_outer_knuckle", "left_finger"), - ("xarm_gripper_base_link", "left_inner_knuckle"), - ("xarm_gripper_base_link", "right_inner_knuckle"), - ("xarm_gripper_base_link", "left_finger"), - ("xarm_gripper_base_link", "right_finger"), - ("link6", "xarm_gripper_base_link"), - ("link6", "left_outer_knuckle"), - ("link6", "right_outer_knuckle"), -] - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - - -def _base_pose(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> PoseStamped: - return PoseStamped( - position=Vector3(x=x, y=y, z=z), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - -def _coordinator_joint_mapping( - name: str, - dof: int, - *, - joint_prefix: str | None = None, -) -> dict[str, str]: - prefix = f"{name}/" if joint_prefix is None else joint_prefix - if not prefix: - return {} - return {f"{prefix}joint{i}": f"joint{i}" for i in range(1, dof + 1)} - - -def _make_xarm_model_config( - name: str, - dof: int, - *, - add_gripper: bool = True, - x_offset: float = 0.0, - y_offset: float = 0.0, - z_offset: float = 0.0, - pitch: float = 0.0, - joint_prefix: str | None = None, - coordinator_task_name: str | None = None, - tf_extra_links: list[str] | None = None, - home_joints: list[float] | None = None, - pre_grasp_offset: float = 0.10, -) -> RobotModelConfig: - xacro_args = { - "dof": str(dof), - "limited": "true", - "attach_xyz": f"{x_offset} {y_offset} {z_offset}", - "attach_rpy": f"0 {pitch} 0", - } - if add_gripper: - xacro_args["add_gripper"] = "true" - - return RobotModelConfig( - name=name, - model_path=_XARM_MODEL_PATH, - base_pose=_base_pose(x_offset, y_offset, z_offset), - joint_names=[f"joint{i}" for i in range(1, dof + 1)], - end_effector_link="link_tcp" if add_gripper else f"link{dof}", - base_link="link_base", - package_paths=_XARM_PACKAGE_PATHS, - xacro_args=xacro_args, - auto_convert_meshes=True, - collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), - joint_name_mapping=_coordinator_joint_mapping( - name, - dof, - joint_prefix=joint_prefix, - ), - coordinator_task_name=coordinator_task_name or f"traj_{name}", - gripper_hardware_id=name if add_gripper else None, - tf_extra_links=tf_extra_links or [], - home_joints=home_joints or [0.0] * dof, - pre_grasp_offset=pre_grasp_offset, - ) - - -def _make_xarm6_model_config( - name: str = "arm", - **kwargs: Any, -) -> RobotModelConfig: - return _make_xarm_model_config(name, 6, **kwargs) - - -def _make_xarm7_model_config( - name: str = "arm", - **kwargs: Any, -) -> RobotModelConfig: - return _make_xarm_model_config(name, 7, **kwargs) - - -xarm6_planner_only = ManipulationModule.blueprint( - robots=[_make_xarm6_model_config(name="arm")], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, -) - - -dual_xarm6_planner = ManipulationModule.blueprint( - robots=[ - _make_xarm6_model_config(name="left_arm", y_offset=0.5), - _make_xarm6_model_config(name="right_arm", y_offset=-0.5), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, +from dimos.robot.manipulators.xarm.blueprints.agentic import ( + xarm7_planner_coordinator_agent as xarm7_planner_coordinator_agent, + xarm_perception_agent as xarm_perception_agent, + xarm_perception_sim_agent as xarm_perception_sim_agent, ) - - -# Single XArm7 planner + coordinator (uses real hardware when XARM7_IP is set) -# Usage: XARM7_IP= dimos run xarm7-planner-coordinator -_xarm7_hw = manipulator( - "arm", - 7, - adapter_type="xarm" if global_config.xarm7_ip else "mock", - address=global_config.xarm7_ip, - gripper=True, +from dimos.robot.manipulators.xarm.blueprints.basic import ( + dual_xarm6_planner as dual_xarm6_planner, + xarm6_planner_only as xarm6_planner_only, + xarm7_planner_coordinator as xarm7_planner_coordinator, ) - -xarm7_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[_make_xarm7_model_config(name="arm", add_gripper=True)], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_hw.joints, - priority=10, - ), - ], - ), -) - - -# XArm7 planner + LLM agent for testing base ManipulationModule skills -# No perception — uses the base module's planning + gripper skills only. -# Usage: dimos run coordinator-mock, then dimos run xarm7-planner-coordinator-agent -_BASE_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ -You are a robotic manipulation assistant controlling an xArm7 robot arm. - -Available skills: -- get_robot_state: Get current joint positions, end-effector pose, and gripper state. -- move_to_pose: Move end-effector to ABSOLUTE x, y, z (meters) with optional roll, pitch, yaw (radians). -- move_to_joints: Move to a joint configuration (comma-separated radians). -- open_gripper / close_gripper / set_gripper: Control the gripper. -- go_home: Move to the home/observe position. -- go_init: Return to the startup position. -- reset: Clear a FAULT state and return to IDLE. Use this when a motion fails. - -COORDINATE SYSTEM (world frame, meters): -- X axis = forward (away from the robot base) -- Y axis = left -- Z axis = up -- Z=0 is the robot base level; typical working height is Z = 0.2-0.5 - -CRITICAL WORKFLOW for relative movement requests (e.g. "move 20cm forward"): -1. Call get_robot_state to get the current EE pose. -2. Add the requested offset to the CURRENT position. Example: if EE is at \ -(0.3, 0.0, 0.4) and user says "move 20cm forward", target is (0.5, 0.0, 0.4). -3. Call move_to_pose with the computed ABSOLUTE target. -NEVER pass only the offset as coordinates — that would send the robot to near-origin. - -ERROR RECOVERY: If a motion fails or the state becomes FAULT, call reset before retrying. -""" - -xarm7_planner_coordinator_agent = autoconnect( - xarm7_planner_coordinator, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_BASE_MANIPULATION_AGENT_SYSTEM_PROMPT), -) - - -# XArm7 with eye-in-hand RealSense camera for perception-based manipulation -# TF chain: world → link7 (ManipulationModule) → camera_link (RealSense) -# Usage: dimos run coordinator-mock, then dimos run xarm-perception -_XARM_PERCEPTION_CAMERA_TRANSFORM = Transform( - translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), - rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw -) - -xarm_perception = autoconnect( - PickAndPlaceModule.blueprint( - robots=[ - _make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - ) - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - floor_z=-0.02, - ), - RealSenseCamera.blueprint( - base_frame_id="link7", - base_transform=_XARM_PERCEPTION_CAMERA_TRANSFORM, - ), - ObjectSceneRegistrationModule.blueprint( - target_frame="world", - distance_threshold=0.08, - min_detections_for_permanent=3, - max_distance=1.0, - use_aabb=True, - max_obstacle_width=0.06, - ), -).global_config(n_workers=4) - - -# XArm7 perception + LLM agent for agentic manipulation. -# Skills (pick, place, move_to_pose, etc.) auto-register with the agent's SkillCoordinator. -# Usage: XARM7_IP= dimos run coordinator-xarm7 xarm-perception-agent -_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ -You are a robotic manipulation assistant controlling an xArm7 robot arm with an \ -eye-in-hand RealSense camera and a gripper. - -# Skills - -## Perception -- **look**: Quick snapshot of objects visible from the current camera pose. Does NOT \ -move the arm. Example: "what do you see?", "what's on the table?" -- **scan_objects**: Full scan — moves the arm to the init position for a clear view, \ -then refreshes detections. Use before pick/place, after a failed grasp, or when the \ -user explicitly asks to scan. Example: "scan the table", "what objects are there?" - -## Pick & Place -- **pick **: Pick up a detected object by name. Use the EXACT name from \ -look/scan_objects output. When duplicates exist, pass the object_id shown in brackets \ -(e.g. [id=abc12345]). Example: "pick the cup", "grab the spray can" -- **place **: Place a held object at explicit world-frame coordinates. \ -Example: "place it at 0.4, 0.3, 0.1" -- **drop_on **: Drop a held object onto another detected object. \ -Automatically compensates for camera occlusion. Example: "drop it in the bowl", \ -"put it on the box" -- **place_back**: Return a held object to its original pick position. -- **pick_and_place **: Pick then place in one command. - -## Motion -- **move_to_pose [roll pitch yaw]**: Move end-effector to an absolute \ -world-frame pose (meters / radians). -- **move_to_joints **: Move to a joint configuration (radians). -- **go_home**: Move to the home/observe position. -- **go_init**: Return to the startup position. Use after pick/place as a safe resting pose. - -## Gripper -- **open_gripper / close_gripper / set_gripper**: Direct gripper control. - -## Status & Recovery -- **get_robot_state**: Current joint positions, end-effector pose, and gripper state. -- **get_scene_info**: Full robot state, detected objects, and scene overview. -- **reset**: Clear a FAULT state and return to IDLE. Available as both a skill and RPC. -- **clear_perception_obstacles**: Remove detected obstacles from the planning world. \ -Use when planning fails with COLLISION_AT_START. - -# Choosing look vs scan_objects -- "what can you see?" / "what's there?" → **look** (instant, no movement) -- "scan the scene" / before pick-and-place → **scan_objects** (thorough, moves arm) -- If objects were ALREADY detected by a previous look, do NOT scan again — just proceed. - -# Rules -- Use the EXACT object name from detection output. Do NOT substitute similar names \ -(e.g. if detection says "spray can", do not use "grinder"). -- "drop it in/on [object]" → use **drop_on**. "place it at [coords]" → use **place**. -- "bring it back" → pick, then **go_init**. Do NOT place randomly. -- "bring it to me" / "hand it over" → pick, then move toward user (≈ X=0, Y=0.5). -- NEVER open the gripper while holding an object unless the user asks or you are \ -executing place/drop_on. The gripper stays closed during movement. -- After pick or place, return to init with **go_init** unless another action follows. - -# Coordinate System -World frame (meters): X = forward, Y = left, Z = up. Z = 0 is robot base. -Typical working area: X 0.3-0.7, Y -0.5 to 0.5, Z 0.05-0.5. - -# Error Recovery -If planning fails with COLLISION_AT_START: call **clear_perception_obstacles**, then \ -**reset**, then retry. -""" - -xarm_perception_agent = autoconnect( - xarm_perception, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT), -) - - -# Sim perception: MujocoSimModule owns the MujocoEngine and publishes both -# camera streams and joint state via shared memory. -# ShmMujocoAdapter attaches to the same SHM buffers by MJCF path. -_xarm7_sim_home = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] -_xarm7_sim_hw = manipulator( - "arm", - 7, - adapter_type="sim_mujoco", - address=str(XARM7_SIM_PATH), - gripper=True, - home_joints=_xarm7_sim_home, -) - -xarm_perception_sim = autoconnect( - PickAndPlaceModule.blueprint( - robots=[ - _make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - home_joints=_xarm7_sim_home, - pre_grasp_offset=0.05, - ) - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - MujocoSimModule.blueprint( - address=str(XARM7_SIM_PATH), - headless=False, - dof=7, - camera_name="wrist_camera", - base_frame_id="link7", - ), - ObjectSceneRegistrationModule.blueprint(target_frame="world"), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_sim_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_sim_hw.joints, - priority=10, - ), - ], - ), - RerunBridgeModule.blueprint(), -) - - -xarm_perception_sim_agent = autoconnect( - xarm_perception_sim, - McpServer.blueprint(), - McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT), +from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception as xarm_perception +from dimos.robot.manipulators.xarm.blueprints.simulation import ( + xarm_perception_sim as xarm_perception_sim, ) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 291af8540e..5ba30bdd8c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -18,31 +18,31 @@ all_blueprints = { "alfred-nav": "dimos.robot.diy.alfred.blueprints.alfred_nav:alfred_nav", "coordinator-basic": "dimos.control.blueprints.basic:coordinator_basic", - "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", - "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", - "coordinator-combined-xarm6": "dimos.control.blueprints.teleop:coordinator_combined_xarm6", - "coordinator-dual-mock": "dimos.control.blueprints.dual:coordinator_dual_mock", - "coordinator-dual-xarm": "dimos.control.blueprints.dual:coordinator_dual_xarm", + "coordinator-cartesian-ik-mock": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_cartesian_ik_mock", + "coordinator-cartesian-ik-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_cartesian_ik_piper", + "coordinator-combined-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_combined_xarm6", + "coordinator-dual-mock": "dimos.robot.manipulators.common.mock:coordinator_dual_mock", + "coordinator-dual-xarm": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", "coordinator-flowbase-nav": "dimos.control.blueprints.mobile:coordinator_flowbase_nav", "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", - "coordinator-mock": "dimos.control.blueprints.basic:coordinator_mock", + "coordinator-mock": "dimos.robot.manipulators.common.mock:coordinator_mock", "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", - "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_bimanual", - "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_left", - "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_mock", - "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_right", - "coordinator-piper": "dimos.control.blueprints.basic:coordinator_piper", - "coordinator-piper-xarm": "dimos.control.blueprints.dual:coordinator_piper_xarm", - "coordinator-servo-xarm6": "dimos.control.blueprints.teleop:coordinator_servo_xarm6", - "coordinator-teleop-dual": "dimos.control.blueprints.teleop:coordinator_teleop_dual", - "coordinator-teleop-piper": "dimos.control.blueprints.teleop:coordinator_teleop_piper", - "coordinator-teleop-xarm6": "dimos.control.blueprints.teleop:coordinator_teleop_xarm6", - "coordinator-teleop-xarm7": "dimos.control.blueprints.teleop:coordinator_teleop_xarm7", - "coordinator-velocity-xarm6": "dimos.control.blueprints.teleop:coordinator_velocity_xarm6", - "coordinator-xarm6": "dimos.control.blueprints.basic:coordinator_xarm6", - "coordinator-xarm7": "dimos.control.blueprints.basic:coordinator_xarm7", + "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_bimanual", + "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_left", + "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_mock", + "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_right", + "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper", + "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm", + "coordinator-servo-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_servo_xarm6", + "coordinator-teleop-dual": "dimos.robot.manipulators.common.mixed:coordinator_teleop_dual", + "coordinator-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_teleop_piper", + "coordinator-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm6", + "coordinator-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm7", + "coordinator-velocity-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_velocity_xarm6", + "coordinator-xarm6": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm6", + "coordinator-xarm7": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm7", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", @@ -58,13 +58,13 @@ "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", - "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", - "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints:keyboard_teleop_a750", - "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", - "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm_mock", - "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints:keyboard_teleop_piper", - "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm6", - "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm7", + "dual-xarm6-planner": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner", + "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", + "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm", + "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_mock", + "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:keyboard_teleop_piper", + "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm6", + "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm7", "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", "mid360-fastlio-ray-trace": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_ray_trace", @@ -72,8 +72,8 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "mid360-pointlio": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio", "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", - "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", - "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", + "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", + "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", @@ -123,13 +123,13 @@ "unitree-go2-webrtc-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_keyboard_teleop:unitree_go2_webrtc_keyboard_teleop", "unitree-go2-webrtc-rage-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_webrtc_rage_keyboard_teleop:unitree_go2_webrtc_rage_keyboard_teleop", "unity-sim": "dimos.simulation.unity.blueprint:unity_sim", - "xarm-perception": "dimos.manipulation.blueprints:xarm_perception", - "xarm-perception-agent": "dimos.manipulation.blueprints:xarm_perception_agent", - "xarm-perception-sim": "dimos.manipulation.blueprints:xarm_perception_sim", - "xarm-perception-sim-agent": "dimos.manipulation.blueprints:xarm_perception_sim_agent", - "xarm6-planner-only": "dimos.manipulation.blueprints:xarm6_planner_only", - "xarm7-planner-coordinator": "dimos.manipulation.blueprints:xarm7_planner_coordinator", - "xarm7-planner-coordinator-agent": "dimos.manipulation.blueprints:xarm7_planner_coordinator_agent", + "xarm-perception": "dimos.robot.manipulators.xarm.blueprints.perception:xarm_perception", + "xarm-perception-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_perception_agent", + "xarm-perception-sim": "dimos.robot.manipulators.xarm.blueprints.simulation:xarm_perception_sim", + "xarm-perception-sim-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm_perception_sim_agent", + "xarm6-planner-only": "dimos.robot.manipulators.xarm.blueprints.basic:xarm6_planner_only", + "xarm7-planner-coordinator": "dimos.robot.manipulators.xarm.blueprints.basic:xarm7_planner_coordinator", + "xarm7-planner-coordinator-agent": "dimos.robot.manipulators.xarm.blueprints.agentic:xarm7_planner_coordinator_agent", } diff --git a/dimos/robot/manipulators/_modeling.py b/dimos/robot/manipulators/_modeling.py new file mode 100644 index 0000000000..852a484b63 --- /dev/null +++ b/dimos/robot/manipulators/_modeling.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Small helpers for manipulator planning model configs.""" + +from __future__ import annotations + +from typing import TypeAlias + +from dimos.manipulation.planning.spec.models import RobotName +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +DegreesOfFreedom: TypeAlias = int +JointPrefix: TypeAlias = str +UrdfJointPrefix: TypeAlias = str +UrdfJointName: TypeAlias = str +CoordinatorJointName: TypeAlias = str +JointNameMapping: TypeAlias = dict[CoordinatorJointName, UrdfJointName] + + +def base_pose(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> PoseStamped: + return PoseStamped( + position=Vector3(x=x, y=y, z=z), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ) + + +def joint_names( + dof: DegreesOfFreedom, + *, + prefix: JointPrefix = "joint", +) -> list[UrdfJointName]: + return [f"{prefix}{i}" for i in range(1, dof + 1)] + + +def coordinator_joint_mapping( + name: RobotName, + dof: DegreesOfFreedom, + *, + joint_prefix: JointPrefix | None = None, + urdf_joint_prefix: UrdfJointPrefix = "", +) -> JointNameMapping: + prefix = f"{name}/" if joint_prefix is None else joint_prefix + if not prefix: + return {} + return {f"{prefix}joint{i}": f"{urdf_joint_prefix}joint{i}" for i in range(1, dof + 1)} diff --git a/dimos/robot/manipulators/a750/blueprints.py b/dimos/robot/manipulators/a750/blueprints.py deleted file mode 100644 index 408f2a4a83..0000000000 --- a/dimos/robot/manipulators/a750/blueprints.py +++ /dev/null @@ -1,119 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Keyboard teleop blueprint for the A-750 arm. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-a750 -""" - -import math -from pathlib import Path - -from dimos.control.blueprints._hardware import A750_FK_MODEL, a750 -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -A750_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("base_link", "link1"), - ("base_link", "link2"), - ("left_finger_link", "link3"), - ("left_finger_link", "link4"), - ("left_finger_link", "link5"), - ("left_finger_link", "link6"), - ("left_finger_link", "right_finger_link"), - ("link1", "link2"), - ("link2", "link3"), - ("link2", "link4"), - ("link3", "link4"), - ("link3", "link5"), - ("link3", "right_finger_link"), - ("link4", "link5"), - ("link4", "link6"), - ("link4", "right_finger_link"), - ("link5", "link6"), - ("link5", "right_finger_link"), - ("link6", "right_finger_link"), -] - -_A750_MODEL_PATH = LfsPath("a750_description") / "urdf/a750_rev1.urdf" -_A750_HOME_JOINTS = [0.0, 0.0, -math.radians(90), 0.0, 0.0, 0.0] -_A750_PACKAGE_PATHS: dict[str, Path] = { - "a750_description": LfsPath("a750_description"), - "a750_gazebo": LfsPath("a750_description"), -} - - -def _a750_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=_A750_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, 7)], - end_effector_link="gripper_base", - base_link="base_link", - package_paths=_A750_PACKAGE_PATHS, - auto_convert_meshes=True, - collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, 7)}, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm", - home_joints=_A750_HOME_JOINTS, - ) - - -_a750_hw = a750("arm", mock_without_address=True) - -# A-750 6-DOF mock sim + keyboard teleop + Drake visualization -keyboard_teleop_a750 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=A750_FK_MODEL, - ee_joint_id=6, - home_joints=_A750_HOME_JOINTS, - joint_names=_a750_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_a750_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_a750_hw.joints, - priority=10, - params={"model_path": A750_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_a750_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py new file mode 100644 index 0000000000..873a01817f --- /dev/null +++ b/dimos/robot/manipulators/a750/blueprints/teleop.py @@ -0,0 +1,51 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""A-750 teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.a750.config import ( + A750_FK_MODEL, + A750_HOME_JOINTS, + a750_hardware, + make_a750_model_config, +) +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_a750_hw = a750_hardware("arm", mock_without_address=True) + +keyboard_teleop_a750 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=A750_FK_MODEL, + ee_joint_id=6, + home_joints=A750_HOME_JOINTS, + joint_names=_a750_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_a750_hw], + tasks=[cartesian_ik_task(_a750_hw, model_path=A750_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_a750_model_config()], + visualization={"backend": "meshcat"}, + ), +) diff --git a/dimos/robot/manipulators/a750/config.py b/dimos/robot/manipulators/a750/config.py new file mode 100644 index 0000000000..32bc57409a --- /dev/null +++ b/dimos/robot/manipulators/a750/config.py @@ -0,0 +1,123 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""A-750 planning model configuration helpers.""" + +from __future__ import annotations + +import math +from pathlib import Path + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +A750_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("base_link", "link1"), + ("base_link", "link2"), + ("left_finger_link", "link3"), + ("left_finger_link", "link4"), + ("left_finger_link", "link5"), + ("left_finger_link", "link6"), + ("left_finger_link", "right_finger_link"), + ("link1", "link2"), + ("link2", "link3"), + ("link2", "link4"), + ("link3", "link4"), + ("link3", "link5"), + ("link3", "right_finger_link"), + ("link4", "link5"), + ("link4", "link6"), + ("link4", "right_finger_link"), + ("link5", "link6"), + ("link5", "right_finger_link"), + ("link6", "right_finger_link"), +] + +A750_HOME_JOINTS = [0.0, 0.0, -math.radians(90), 0.0, 0.0, 0.0] +A750_MODEL_PATH = LfsPath("a750_description") / "urdf/a750_rev1.urdf" +A750_FK_MODEL = LfsPath("a750_description/urdf/a750_rev1_no_gripper.urdf") +A750_PACKAGE_PATHS: dict[str, Path] = { + "a750_description": LfsPath("a750_description"), + "a750_gazebo": LfsPath("a750_description"), +} + + +def make_a750_hardware( + hw_id: str = "arm", + *, + adapter_type: str = "a750", + address: str | None = None, + gripper: bool = True, + auto_enable: bool = True, + home_joints: list[float] | None = None, +) -> HardwareComponent: + joints = make_joints(hw_id, 6) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=joints, + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/finger"] if gripper else [], + adapter_kwargs={"initial_positions": home_joints or A750_HOME_JOINTS}, + ) + + +def a750_hardware(hw_id: str = "arm", *, mock_without_address: bool = False) -> HardwareComponent: + if mock_without_address and not global_config.device_path: + return make_a750_hardware( + hw_id, + adapter_type="mock", + address=None, + ) + return make_a750_hardware( + hw_id, + address=global_config.device_path or "/dev/ttyACM0", + ) + + +def make_a750_model_config( + name: str = "arm", + *, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, +) -> RobotModelConfig: + dof = 6 + return RobotModelConfig( + name=name, + model_path=A750_MODEL_PATH, + base_pose=base_pose(), + joint_names=joint_names(dof), + end_effector_link="gripper_base", + base_link="base_link", + package_paths=A750_PACKAGE_PATHS, + auto_convert_meshes=True, + collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name, + home_joints=A750_HOME_JOINTS, + ) diff --git a/dimos/robot/manipulators/common/agent_prompts.py b/dimos/robot/manipulators/common/agent_prompts.py new file mode 100644 index 0000000000..a799455861 --- /dev/null +++ b/dimos/robot/manipulators/common/agent_prompts.py @@ -0,0 +1,109 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""System prompts used by manipulation agent blueprints.""" + +BASE_MANIPULATION_AGENT_SYSTEM_PROMPT = """\ +You are a robotic manipulation assistant controlling an xArm7 robot arm. + +Available skills: +- get_robot_state: Get current joint positions, end-effector pose, and gripper state. +- move_to_pose: Move end-effector to ABSOLUTE x, y, z (meters) with optional roll, pitch, yaw (radians). +- move_to_joints: Move to a joint configuration (comma-separated radians). +- open_gripper / close_gripper / set_gripper: Control the gripper. +- go_home: Move to the home/observe position. +- go_init: Return to the startup position. +- reset: Clear a FAULT state and return to IDLE. Use this when a motion fails. + +COORDINATE SYSTEM (world frame, meters): +- X axis = forward (away from the robot base) +- Y axis = left +- Z axis = up +- Z=0 is the robot base level; typical working height is Z = 0.2-0.5 + +CRITICAL WORKFLOW for relative movement requests (e.g. "move 20cm forward"): +1. Call get_robot_state to get the current EE pose. +2. Add the requested offset to the CURRENT position. Example: if EE is at \ +(0.3, 0.0, 0.4) and user says "move 20cm forward", target is (0.5, 0.0, 0.4). +3. Call move_to_pose with the computed ABSOLUTE target. +NEVER pass only the offset as coordinates — that would send the robot to near-origin. + +ERROR RECOVERY: If a motion fails or the state becomes FAULT, call reset before retrying. +""" + +MANIPULATION_AGENT_SYSTEM_PROMPT = """\ +You are a robotic manipulation assistant controlling an xArm7 robot arm with an \ +eye-in-hand RealSense camera and a gripper. + +# Skills + +## Perception +- **look**: Quick snapshot of objects visible from the current camera pose. Does NOT \ +move the arm. Example: "what do you see?", "what's on the table?" +- **scan_objects**: Full scan — moves the arm to the init position for a clear view, \ +then refreshes detections. Use before pick/place, after a failed grasp, or when the \ +user explicitly asks to scan. Example: "scan the table", "what objects are there?" + +## Pick & Place +- **pick **: Pick up a detected object by name. Use the EXACT name from \ +look/scan_objects output. When duplicates exist, pass the object_id shown in brackets \ +(e.g. [id=abc12345]). Example: "pick the cup", "grab the spray can" +- **place **: Place a held object at explicit world-frame coordinates. \ +Example: "place it at 0.4, 0.3, 0.1" +- **drop_on **: Drop a held object onto another detected object. \ +Automatically compensates for camera occlusion. Example: "drop it in the bowl", \ +"put it on the box" +- **place_back**: Return a held object to its original pick position. +- **pick_and_place **: Pick then place in one command. + +## Motion +- **move_to_pose [roll pitch yaw]**: Move end-effector to an absolute \ +world-frame pose (meters / radians). +- **move_to_joints **: Move to a joint configuration (radians). +- **go_home**: Move to the home/observe position. +- **go_init**: Return to the startup position. Use after pick/place as a safe resting pose. + +## Gripper +- **open_gripper / close_gripper / set_gripper**: Direct gripper control. + +## Status & Recovery +- **get_robot_state**: Current joint positions, end-effector pose, and gripper state. +- **get_scene_info**: Full robot state, detected objects, and scene overview. +- **reset**: Clear a FAULT state and return to IDLE. Available as both a skill and RPC. +- **clear_perception_obstacles**: Remove detected obstacles from the planning world. \ +Use when planning fails with COLLISION_AT_START. + +# Choosing look vs scan_objects +- "what can you see?" / "what's there?" → **look** (instant, no movement) +- "scan the scene" / before pick-and-place → **scan_objects** (thorough, moves arm) +- If objects were ALREADY detected by a previous look, do NOT scan again — just proceed. + +# Rules +- Use the EXACT object name from detection output. Do NOT substitute similar names \ +(e.g. if detection says "spray can", do not use "grinder"). +- "drop it in/on [object]" → use **drop_on**. "place it at [coords]" → use **place**. +- "bring it back" → pick, then **go_init**. Do NOT place randomly. +- "bring it to me" / "hand it over" → pick, then move toward user (≈ X=0, Y=0.5). +- NEVER open the gripper while holding an object unless the user asks or you are \ +executing place/drop_on. The gripper stays closed during movement. +- After pick or place, return to init with **go_init** unless another action follows. + +# Coordinate System +World frame (meters): X = forward, Y = left, Z = up. Z = 0 is robot base. +Typical working area: X 0.3-0.7, Y -0.5 to 0.5, Z 0.05-0.5. + +# Error Recovery +If planning fails with COLLISION_AT_START: call **clear_perception_obstacles**, then \ +**reset**, then retry. +""" diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py new file mode 100644 index 0000000000..1f85998a67 --- /dev/null +++ b/dimos/robot/manipulators/common/blueprints.py @@ -0,0 +1,128 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Small blueprint helpers shared by manipulator stacks.""" + +from __future__ import annotations + +from collections.abc import Sequence +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import Blueprint +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators.common.topics import ( + CARTESIAN_IK_TASK_NAME, + COORDINATOR_FRAME_ID, + DEFAULT_TRAJECTORY_TASK_NAME, + trajectory_task_name, +) + + +def trajectory_task( + hardware: HardwareComponent, + *, + name: str | None = None, + priority: int = 10, +) -> TaskConfig: + return TaskConfig( + name=name or trajectory_task_name(hardware.hardware_id), + type="trajectory", + joint_names=hardware.joints, + priority=priority, + ) + + +def cartesian_ik_task( + hardware: HardwareComponent, + *, + model_path: Path, + ee_joint_id: int, + name: str = CARTESIAN_IK_TASK_NAME, + priority: int = 10, +) -> TaskConfig: + return TaskConfig( + name=name, + type="cartesian_ik", + joint_names=hardware.joints, + priority=priority, + params={"model_path": model_path, "ee_joint_id": ee_joint_id}, + ) + + +def teleop_ik_task( + hardware: HardwareComponent, + *, + model_path: Path, + ee_joint_id: int, + hand: str, + name: str, + priority: int = 10, + params: dict[str, Any] | None = None, +) -> TaskConfig: + task_params: dict[str, Any] = { + "model_path": model_path, + "ee_joint_id": ee_joint_id, + "hand": hand, + } + if params: + task_params.update(params) + return TaskConfig( + name=name, + type="teleop_ik", + joint_names=hardware.joints, + priority=priority, + params=task_params, + ) + + +def coordinator( + *, + hardware: Sequence[HardwareComponent] = (), + tasks: Sequence[TaskConfig] = (), + tick_rate: float = 100.0, + publish_joint_state: bool = True, + joint_state_frame_id: str = COORDINATOR_FRAME_ID, +) -> Blueprint: + return ControlCoordinator.blueprint( + tick_rate=tick_rate, + publish_joint_state=publish_joint_state, + joint_state_frame_id=joint_state_frame_id, + hardware=list(hardware), + tasks=list(tasks), + ) + + +def planner( + *, + robots: Sequence[RobotModelConfig], + planning_timeout: float = 10.0, + visualization: dict[str, Any] | None = None, + **kwargs: Any, +) -> Blueprint: + return ManipulationModule.blueprint( + robots=list(robots), + planning_timeout=planning_timeout, + visualization=visualization or {"backend": "meshcat"}, + **kwargs, + ) + + +def default_trajectory_task_name(hardware_id: str) -> str: + if hardware_id == "arm": + return DEFAULT_TRAJECTORY_TASK_NAME + return trajectory_task_name(hardware_id) diff --git a/dimos/control/blueprints/dual.py b/dimos/robot/manipulators/common/mixed.py similarity index 50% rename from dimos/control/blueprints/dual.py rename to dimos/robot/manipulators/common/mixed.py index 07450bbefb..05cecaa268 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/robot/manipulators/common/mixed.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,83 +12,70 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Dual-arm coordinator blueprints with trajectory control. - -Usage: - dimos run coordinator-dual-mock # Mock 7+6 DOF arms - dimos run coordinator-dual-xarm # XArm7 left + XArm6 right - dimos run coordinator-piper-xarm # XArm6 + Piper -""" +"""Mixed-manipulator coordinator blueprints.""" from __future__ import annotations -from dimos.control.blueprints._hardware import manipulator, mock_arm from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.global_config import global_config +from dimos.robot.manipulators.piper.config import PIPER_FK_MODEL, make_piper_hardware +from dimos.robot.manipulators.xarm.config import XARM6_FK_MODEL, make_xarm_hardware -# Dual mock arms (7-DOF left, 6-DOF right) -_mock_left = mock_arm("left_arm", 7) -_mock_right = mock_arm("right_arm", 6) - -coordinator_dual_mock = ControlCoordinator.blueprint( - hardware=[_mock_left, _mock_right], - tasks=[ - TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10), - TaskConfig( - name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10 - ), - ], -) - -# Dual XArm (XArm7 left, XArm6 right) -_xarm7_left = manipulator( - "left_arm", - 7, - adapter_type="xarm", - address=global_config.xarm7_ip, -) -_xarm6_right = manipulator( - "right_arm", +_xarm6_dual = make_xarm_hardware( + "xarm_arm", 6, adapter_type="xarm", address=global_config.xarm6_ip, ) +_piper_dual = make_piper_hardware( + "piper_arm", + adapter_type="piper", + address=global_config.can_port or "can0", + gripper=True, +) -coordinator_dual_xarm = ControlCoordinator.blueprint( - hardware=[_xarm7_left, _xarm6_right], +coordinator_piper_xarm = ControlCoordinator.blueprint( + hardware=[_xarm6_dual, _piper_dual], tasks=[ TaskConfig( - name="traj_left", type="trajectory", joint_names=_xarm7_left.joints, priority=10 + name="traj_xarm", type="trajectory", joint_names=_xarm6_dual.joints, priority=10 ), TaskConfig( - name="traj_right", type="trajectory", joint_names=_xarm6_right.joints, priority=10 + name="traj_piper", type="trajectory", joint_names=_piper_dual.joints, priority=10 ), ], ) -# Dual arm (XArm6 + Piper) -_xarm6_dual = manipulator( +_xarm6_teleop_hw = make_xarm_hardware( "xarm_arm", 6, adapter_type="xarm", address=global_config.xarm6_ip, + gripper=True, ) -_piper_dual = manipulator( +_piper_teleop_hw = make_piper_hardware( "piper_arm", - 6, adapter_type="piper", address=global_config.can_port or "can0", gripper=True, ) -coordinator_piper_xarm = ControlCoordinator.blueprint( - hardware=[_xarm6_dual, _piper_dual], +coordinator_teleop_dual = ControlCoordinator.blueprint( + hardware=[_xarm6_teleop_hw, _piper_teleop_hw], tasks=[ TaskConfig( - name="traj_xarm", type="trajectory", joint_names=_xarm6_dual.joints, priority=10 + name="teleop_xarm", + type="teleop_ik", + joint_names=_xarm6_teleop_hw.joints, + priority=10, + params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"}, ), TaskConfig( - name="traj_piper", type="trajectory", joint_names=_piper_dual.joints, priority=10 + name="teleop_piper", + type="teleop_ik", + joint_names=_piper_teleop_hw.joints, + priority=10, + params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"}, ), ], ) diff --git a/dimos/robot/manipulators/common/mock.py b/dimos/robot/manipulators/common/mock.py new file mode 100644 index 0000000000..c48efc035b --- /dev/null +++ b/dimos/robot/manipulators/common/mock.py @@ -0,0 +1,62 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Mock manipulator coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig + +_mock_hw = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", +) + +coordinator_mock = ControlCoordinator.blueprint( + hardware=[_mock_hw], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=_mock_hw.joints, + priority=10, + ) + ], +) + +_mock_left = HardwareComponent( + hardware_id="left_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("left_arm", 7), + adapter_type="mock", +) +_mock_right = HardwareComponent( + hardware_id="right_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("right_arm", 6), + adapter_type="mock", +) + +coordinator_dual_mock = ControlCoordinator.blueprint( + hardware=[_mock_left, _mock_right], + tasks=[ + TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10), + TaskConfig( + name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10 + ), + ], +) diff --git a/dimos/robot/manipulators/common/sim.py b/dimos/robot/manipulators/common/sim.py new file mode 100644 index 0000000000..d613cd9821 --- /dev/null +++ b/dimos/robot/manipulators/common/sim.py @@ -0,0 +1,31 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Simulation helpers for manipulator blueprints.""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config + + +def mujoco_if_sim(sim_path: str | Path, dof: int) -> tuple[Blueprint, ...]: + if not global_config.simulation: + return () + + from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule + + return (MujocoSimModule.blueprint(address=str(sim_path), headless=False, dof=dof),) diff --git a/dimos/robot/manipulators/common/topics.py b/dimos/robot/manipulators/common/topics.py new file mode 100644 index 0000000000..aa0ed70206 --- /dev/null +++ b/dimos/robot/manipulators/common/topics.py @@ -0,0 +1,30 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared control naming conventions for manipulator blueprints.""" + +from __future__ import annotations + +from typing import TypeAlias + +FrameId: TypeAlias = str +TaskName: TypeAlias = str + +COORDINATOR_FRAME_ID: FrameId = "coordinator" +CARTESIAN_IK_TASK_NAME: TaskName = "cartesian_ik_arm" +DEFAULT_TRAJECTORY_TASK_NAME: TaskName = "traj_arm" + + +def trajectory_task_name(hardware_id: str) -> TaskName: + return f"traj_{hardware_id}" diff --git a/dimos/robot/manipulators/openarm/blueprints.py b/dimos/robot/manipulators/openarm/blueprints.py deleted file mode 100644 index 84a8a0489a..0000000000 --- a/dimos/robot/manipulators/openarm/blueprints.py +++ /dev/null @@ -1,289 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""OpenArm blueprints. Flip LEFT_CAN / RIGHT_CAN below if arms come up swapped.""" - -from __future__ import annotations - -from pathlib import Path -from typing import Any - -from dimos.control.components import HardwareComponent, HardwareType -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] - -_OPENARM_PKG = LfsPath("openarm_description") -_OPENARM_LEFT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" -_OPENARM_RIGHT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_V10_FK_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" -_OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": _OPENARM_PKG} - - -def _base_pose() -> PoseStamped: - return PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - -def _validate_side(side: str) -> None: - if side not in ("left", "right"): - raise ValueError(f"side must be 'left' or 'right', got {side!r}") - - -def _openarm_joints(side: str) -> list[str]: - _validate_side(side) - return [f"openarm_{side}_joint{i}" for i in range(1, 8)] - - -def _openarm_hardware( - side: str, - name: str | None = None, - *, - adapter_type: str = "mock", - address: str | None = None, - adapter_kwargs: dict[str, Any] | None = None, -) -> HardwareComponent: - _validate_side(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) - return HardwareComponent( - hardware_id=name or f"{side}_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=_openarm_joints(side), - adapter_type=adapter_type, - address=address, - adapter_kwargs=kwargs, - ) - - -def _openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: - _validate_side(side) - resolved_name = name or f"{side}_arm" - return RobotModelConfig( - name=resolved_name, - model_path=_OPENARM_LEFT_MODEL if side == "left" else _OPENARM_RIGHT_MODEL, - base_pose=_base_pose(), - joint_names=_openarm_joints(side), - end_effector_link=f"openarm_{side}_link7", - base_link="openarm_body_link0", - package_paths=_OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - coordinator_task_name=f"traj_{resolved_name}", - home_joints=[0.0] * 7, - ) - - -def _openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return TaskConfig( - name=name or f"traj_{hw.hardware_id}", - type="trajectory", - joint_names=hw.joints, - priority=10, - ) - - -def _openarm_single_hardware( - *, - adapter_type: str = "mock", - address: str | None = None, -) -> HardwareComponent: - return _openarm_hardware( - "left", - name="arm", - adapter_type=adapter_type, - address=address, - ) - - -def _openarm_single_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=OPENARM_V10_FK_MODEL, - base_pose=_base_pose(), - joint_names=_openarm_joints("left"), - end_effector_link="openarm_left_link7", - base_link="openarm_body_link0", - package_paths=_OPENARM_PACKAGE_PATHS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - coordinator_task_name="traj_arm", - home_joints=[0.0] * 7, - ) - - -# Mock bimanual: no hardware, great for verifying wiring. -_mock_left = _openarm_hardware(side="left") -_mock_right = _openarm_hardware(side="right") - -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[_mock_left, _mock_right], - tasks=[ - _openarm_task(_mock_left), - _openarm_task(_mock_right), - ], -) - -# Single-arm hardware blueprints (first real bring-up targets). -# CAN interface each physical arm is on. Linux assigns can0/can1 in USB -# enumeration order which is not guaranteed stable; if swapped, flip these. -LEFT_CAN = "can1" -RIGHT_CAN = "can0" - -# Flip to False to skip the CTRL_MODE=MIT write at connect-time. Leave True for -# normal operation; it is idempotent and ensures motors are in the expected mode. -AUTO_SET_MIT_MODE = True - -_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} -_left_hw = _openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, -) -_right_hw = _openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=_ADAPTER_KWARGS, -) - -coordinator_openarm_left = ControlCoordinator.blueprint( - hardware=[_left_hw], - tasks=[_openarm_task(_left_hw)], -) - -coordinator_openarm_right = ControlCoordinator.blueprint( - hardware=[_right_hw], - tasks=[_openarm_task(_right_hw)], -) - -coordinator_openarm_bimanual = ControlCoordinator.blueprint( - hardware=[_left_hw, _right_hw], - tasks=[ - _openarm_task(_left_hw), - _openarm_task(_right_hw), - ], -) - - -# Planner + coordinator (mock): Drake plans, mock adapters execute. -openarm_mock_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _openarm_model_config("left"), - _openarm_model_config("right"), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - hardware=[_mock_left, _mock_right], - tasks=[ - _openarm_task(_mock_left), - _openarm_task(_mock_right), - ], - ), -) - -# Planner + coordinator (real hw): plan and execute on both arms. -openarm_planner_coordinator = autoconnect( - ManipulationModule.blueprint( - robots=[ - _openarm_model_config("left"), - _openarm_model_config("right"), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, - ), - ControlCoordinator.blueprint( - hardware=[_left_hw, _right_hw], - tasks=[ - _openarm_task(_left_hw), - _openarm_task(_right_hw), - ], - ), -) - - -# Keyboard teleop (single arm, mock). -_teleop_hw = _openarm_single_hardware() - -keyboard_teleop_openarm_mock = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - joint_names=_teleop_hw.joints, - ), - ControlCoordinator.blueprint( - hardware=[_teleop_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_teleop_hw.joints, - priority=10, - params={"model_path": OPENARM_V10_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), -) - -# Keyboard teleop (single arm, real hw on can0). -_teleop_real_hw = _openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) - -keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - joint_names=_teleop_real_hw.joints, - ), - ControlCoordinator.blueprint( - hardware=[_teleop_real_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_teleop_real_hw.joints, - priority=10, - params={"model_path": OPENARM_V10_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py new file mode 100644 index 0000000000..3d0ef80f8e --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -0,0 +1,74 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Basic OpenArm coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.robot.manipulators.common.blueprints import trajectory_task +from dimos.robot.manipulators.openarm.config import ( + LEFT_CAN, + OPENARM_ADAPTER_KWARGS, + RIGHT_CAN, + openarm_hardware, +) + + +def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: + return trajectory_task(hw, name=name) + + +mock_left = openarm_hardware(side="left") +mock_right = openarm_hardware(side="right") + +coordinator_openarm_mock = ControlCoordinator.blueprint( + hardware=[mock_left, mock_right], + tasks=[ + openarm_task(mock_left), + openarm_task(mock_right), + ], +) + +left_hw = openarm_hardware( + side="left", + address=LEFT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, +) +right_hw = openarm_hardware( + side="right", + address=RIGHT_CAN, + adapter_type="openarm", + adapter_kwargs=OPENARM_ADAPTER_KWARGS, +) + +coordinator_openarm_left = ControlCoordinator.blueprint( + hardware=[left_hw], + tasks=[openarm_task(left_hw)], +) + +coordinator_openarm_right = ControlCoordinator.blueprint( + hardware=[right_hw], + tasks=[openarm_task(right_hw)], +) + +coordinator_openarm_bimanual = ControlCoordinator.blueprint( + hardware=[left_hw, right_hw], + tasks=[ + openarm_task(left_hw), + openarm_task(right_hw), + ], +) diff --git a/dimos/robot/manipulators/openarm/blueprints/planner.py b/dimos/robot/manipulators/openarm/blueprints/planner.py new file mode 100644 index 0000000000..467461b012 --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/planner.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm planner + coordinator blueprints.""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.openarm.blueprints.basic import ( + left_hw, + mock_left, + mock_right, + openarm_task, + right_hw, +) +from dimos.robot.manipulators.openarm.config import openarm_model_config + +openarm_mock_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + coordinator( + hardware=[mock_left, mock_right], + tasks=[ + openarm_task(mock_left), + openarm_task(mock_right), + ], + ), +) + +openarm_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + coordinator( + hardware=[left_hw, right_hw], + tasks=[ + openarm_task(left_hw), + openarm_task(right_hw), + ], + ), +) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py new file mode 100644 index 0000000000..55a477fb73 --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -0,0 +1,71 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm keyboard teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task +from dimos.robot.manipulators.openarm.config import ( + LEFT_CAN, + OPENARM_V10_FK_MODEL, + openarm_single_hardware, + openarm_single_model_config, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_teleop_hw = openarm_single_hardware() + +keyboard_teleop_openarm_mock = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + joint_names=_teleop_hw.joints, + ), + ControlCoordinator.blueprint( + hardware=[_teleop_hw], + tasks=[cartesian_ik_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)], + ), + ManipulationModule.blueprint( + robots=[openarm_single_model_config()], + visualization={"backend": "meshcat"}, + ), +) + +_teleop_real_hw = openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + joint_names=_teleop_real_hw.joints, + ), + ControlCoordinator.blueprint( + hardware=[_teleop_real_hw], + tasks=[ + cartesian_ik_task( + _teleop_real_hw, + model_path=OPENARM_V10_FK_MODEL, + ee_joint_id=7, + ) + ], + ), + ManipulationModule.blueprint( + robots=[openarm_single_model_config()], + visualization={"backend": "meshcat"}, + ), +) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py new file mode 100644 index 0000000000..5306d408bf --- /dev/null +++ b/dimos/robot/manipulators/openarm/config.py @@ -0,0 +1,128 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm hardware and planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import base_pose +from dimos.utils.data import LfsPath + +OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("openarm_left_link5", "openarm_left_link7"), + ("openarm_right_link5", "openarm_right_link7"), +] + +OPENARM_PKG = LfsPath("openarm_description") +OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" +OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" +OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" +OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} + +# Linux assigns can0/can1 in USB enumeration order, which is not guaranteed stable. +# Flip these if physical arms come up swapped. +LEFT_CAN = "can1" +RIGHT_CAN = "can0" + +# Leave true for normal operation; it is idempotent and ensures motors are in +# the expected CTRL_MODE=MIT mode at connect time. +AUTO_SET_MIT_MODE = True +OPENARM_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} + + +def validate_side(side: str) -> None: + if side not in ("left", "right"): + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + + +def openarm_joints(side: str) -> list[str]: + validate_side(side) + return [f"openarm_{side}_joint{i}" for i in range(1, 8)] + + +def openarm_hardware( + side: str, + name: str | None = None, + *, + adapter_type: str = "mock", + address: str | None = None, + adapter_kwargs: dict[str, Any] | None = None, +) -> HardwareComponent: + validate_side(side) + kwargs = {"side": side} + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=name or f"{side}_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=openarm_joints(side), + adapter_type=adapter_type, + address=address, + adapter_kwargs=kwargs, + ) + + +def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: + validate_side(side) + resolved_name = name or f"{side}_arm" + return RobotModelConfig( + name=resolved_name, + model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, + base_pose=base_pose(), + joint_names=openarm_joints(side), + end_effector_link=f"openarm_{side}_link7", + base_link="openarm_body_link0", + package_paths=OPENARM_PACKAGE_PATHS, + collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, + auto_convert_meshes=True, + max_velocity=0.5, + max_acceleration=1.0, + coordinator_task_name=f"traj_{resolved_name}", + home_joints=[0.0] * 7, + ) + + +def openarm_single_hardware( + *, + adapter_type: str = "mock", + address: str | None = None, +) -> HardwareComponent: + return openarm_hardware( + "left", + name="arm", + adapter_type=adapter_type, + address=address, + ) + + +def openarm_single_model_config() -> RobotModelConfig: + return RobotModelConfig( + name="arm", + model_path=OPENARM_V10_FK_MODEL, + base_pose=base_pose(), + joint_names=openarm_joints("left"), + end_effector_link="openarm_left_link7", + base_link="openarm_body_link0", + package_paths=OPENARM_PACKAGE_PATHS, + auto_convert_meshes=True, + max_velocity=0.5, + max_acceleration=1.0, + coordinator_task_name="traj_arm", + home_joints=[0.0] * 7, + ) diff --git a/dimos/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py deleted file mode 100644 index 2ae06e98d5..0000000000 --- a/dimos/robot/manipulators/piper/blueprints.py +++ /dev/null @@ -1,108 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Keyboard teleop blueprint for the Piper arm. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-piper -""" - -from pathlib import Path - -from dimos.control.blueprints._hardware import PIPER_FK_MODEL, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -PIPER_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("gripper_base", "link7"), - ("gripper_base", "link8"), - ("link7", "link8"), - ("link6", "gripper_base"), -] - -_PIPER_MODEL_PATH = LfsPath("piper_description") / "urdf/piper_description.xacro" -_PIPER_PACKAGE_PATHS: dict[str, Path] = { - "piper_description": LfsPath("piper_description"), - "piper_gazebo": LfsPath("piper_description"), -} - - -def _piper_model_config() -> RobotModelConfig: - return RobotModelConfig( - name="arm", - model_path=_PIPER_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, 7)], - end_effector_link="gripper_base", - base_link="base_link", - package_paths=_PIPER_PACKAGE_PATHS, - auto_convert_meshes=True, - collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, 7)}, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm", - home_joints=[0.0] * 6, - ) - - -_piper_hw = manipulator( - "arm", - 6, - adapter_type="piper" if global_config.can_port else "mock", - address=global_config.can_port or "can0", - gripper=True, -) - -# Piper 6-DOF mock sim + keyboard teleop + Drake visualization -keyboard_teleop_piper = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=PIPER_FK_MODEL, - ee_joint_id=6, - joint_names=_piper_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_piper_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_piper_hw.joints, - priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_piper_model_config()], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/piper/blueprints/basic.py b/dimos/robot/manipulators/piper/blueprints/basic.py new file mode 100644 index 0000000000..415c08914d --- /dev/null +++ b/dimos/robot/manipulators/piper/blueprints/basic.py @@ -0,0 +1,39 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Basic Piper coordinator blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.piper.config import PIPER_SIM_PATH, piper_hardware + +_piper_hw = piper_hardware("arm") + +coordinator_piper = autoconnect( + ControlCoordinator.blueprint( + hardware=[_piper_hw], + tasks=[ + TaskConfig( + name="traj_piper", + type="trajectory", + joint_names=_piper_hw.joints, + priority=10, + ) + ], + ), + *mujoco_if_sim(PIPER_SIM_PATH, len(_piper_hw.joints)), +) diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py new file mode 100644 index 0000000000..df3d78d256 --- /dev/null +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -0,0 +1,104 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Piper teleop blueprints.""" + +from __future__ import annotations + +from dimos.control.components import make_gripper_joints +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.piper.config import ( + PIPER_FK_MODEL, + PIPER_SIM_PATH, + make_piper_hardware, + make_piper_model_config, + piper_hardware, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_piper_keyboard_hw = make_piper_hardware( + "arm", + adapter_type="piper" if global_config.can_port else "mock", + address=global_config.can_port or "can0", + gripper=True, +) + +keyboard_teleop_piper = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=PIPER_FK_MODEL, + ee_joint_id=6, + joint_names=_piper_keyboard_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_piper_keyboard_hw], + tasks=[cartesian_ik_task(_piper_keyboard_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_piper_model_config()], + visualization={"backend": "meshcat"}, + ), +) + +_piper_mock_cartesian_hw = make_piper_hardware( + "arm", + gripper=False, +) + +coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( + hardware=[_piper_mock_cartesian_hw], + tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], +) + +_piper_teleop_hw = piper_hardware("arm") + +coordinator_teleop_piper = autoconnect( + ControlCoordinator.blueprint( + hardware=[_piper_teleop_hw], + tasks=[ + teleop_ik_task( + _piper_teleop_hw, + model_path=PIPER_FK_MODEL, + ee_joint_id=6, + hand="left", + name="teleop_piper", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.0, + "gripper_closed_pos": 0.035, + }, + ), + ], + ), + *mujoco_if_sim(PIPER_SIM_PATH, len(_piper_teleop_hw.joints)), +) + +_piper_cartesian_hw = make_piper_hardware( + "arm", + adapter_type="piper", + address=global_config.can_port or "can0", + gripper=True, +) + +coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( + hardware=[_piper_cartesian_hw], + tasks=[cartesian_ik_task(_piper_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], +) diff --git a/dimos/robot/manipulators/piper/config.py b/dimos/robot/manipulators/piper/config.py new file mode 100644 index 0000000000..f99e9589a5 --- /dev/null +++ b/dimos/robot/manipulators/piper/config.py @@ -0,0 +1,131 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Piper planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +PIPER_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("gripper_base", "link7"), + ("gripper_base", "link8"), + ("link7", "link8"), + ("link6", "gripper_base"), +] + +PIPER_MODEL_PATH = LfsPath("piper_description") / "urdf/piper_description.xacro" +PIPER_PACKAGE_PATHS: dict[str, Path] = { + "piper_description": LfsPath("piper_description"), + "piper_gazebo": LfsPath("piper_description"), +} +PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") +PIPER_SIM_PATH = LfsPath("piper/scene.xml") + + +def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: + if home_joints is None: + return {} + return {"initial_positions": home_joints} + + +def make_piper_hardware( + hw_id: str = "arm", + *, + adapter_type: str = "mock", + address: str | None = None, + gripper: bool = True, + auto_enable: bool = True, + adapter_kwargs: dict[str, object] | None = None, + home_joints: list[float] | None = None, +) -> HardwareComponent: + kwargs = _adapter_kwargs(home_joints) + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, 6), + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/gripper"] if gripper else [], + adapter_kwargs=kwargs, + ) + + +def piper_hardware( + hw_id: str = "arm", + *, + gripper: bool = True, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_piper_hardware( + hw_id, + adapter_type="sim_mujoco", + address=str(PIPER_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.can_port or "can0" + if mock_without_address and not global_config.can_port: + return make_piper_hardware(hw_id, gripper=gripper, home_joints=home_joints) + return make_piper_hardware( + hw_id, + adapter_type="piper", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def make_piper_model_config( + name: str = "arm", + *, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, + home_joints: list[float] | None = None, +) -> RobotModelConfig: + dof = 6 + return RobotModelConfig( + name=name, + model_path=PIPER_MODEL_PATH, + base_pose=base_pose(), + joint_names=joint_names(dof), + end_effector_link="gripper_base", + base_link="base_link", + package_paths=PIPER_PACKAGE_PATHS, + auto_convert_meshes=True, + collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name, + home_joints=home_joints or [0.0] * dof, + ) diff --git a/dimos/robot/manipulators/xarm/blueprints.py b/dimos/robot/manipulators/xarm/blueprints.py deleted file mode 100644 index 97b3db042b..0000000000 --- a/dimos/robot/manipulators/xarm/blueprints.py +++ /dev/null @@ -1,161 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Keyboard teleop blueprints for XArm6 and XArm7. - -Launches the ControlCoordinator (mock adapter + CartesianIK), the -ManipulationModule (Drake/Meshcat visualization), and a pygame keyboard -teleop UI — all wired together via autoconnect. - -Usage: - dimos run keyboard-teleop-xarm6 - dimos run keyboard-teleop-xarm7 -""" - -from pathlib import Path - -from dimos.control.blueprints._hardware import XARM6_FK_MODEL, XARM7_FK_MODEL, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config -from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - -XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("right_inner_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "left_outer_knuckle"), - ("right_inner_knuckle", "right_finger"), - ("left_inner_knuckle", "left_finger"), - ("left_finger", "right_finger"), - ("left_outer_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "right_inner_knuckle"), - ("left_outer_knuckle", "right_finger"), - ("right_outer_knuckle", "left_finger"), - ("xarm_gripper_base_link", "left_inner_knuckle"), - ("xarm_gripper_base_link", "right_inner_knuckle"), - ("xarm_gripper_base_link", "left_finger"), - ("xarm_gripper_base_link", "right_finger"), - ("link6", "xarm_gripper_base_link"), - ("link6", "left_outer_knuckle"), - ("link6", "right_outer_knuckle"), -] - - -def _xarm_model_config(dof: int, *, add_gripper: bool = False) -> RobotModelConfig: - xacro_args = { - "dof": str(dof), - "limited": "true", - "attach_xyz": "0.0 0.0 0.0", - "attach_rpy": "0 0.0 0", - } - if add_gripper: - xacro_args["add_gripper"] = "true" - - return RobotModelConfig( - name="arm", - model_path=_XARM_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, dof + 1)], - end_effector_link="link_tcp" if add_gripper else f"link{dof}", - base_link="link_base", - package_paths=_XARM_PACKAGE_PATHS, - xacro_args=xacro_args, - auto_convert_meshes=True, - collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, dof + 1)}, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm" if add_gripper else None, - home_joints=[0.0] * dof, - ) - - -_xarm6_hw = manipulator( - "arm", - 6, - adapter_type="xarm" if global_config.xarm6_ip else "mock", - address=global_config.xarm6_ip, -) -_xarm7_hw = manipulator( - "arm", - 7, - adapter_type="xarm" if global_config.xarm7_ip else "mock", - address=global_config.xarm7_ip, -) - -# XArm6 mock sim + keyboard teleop + Drake visualization -keyboard_teleop_xarm6 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=XARM6_FK_MODEL, - ee_joint_id=6, - joint_names=_xarm6_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm6_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm6_hw.joints, - priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_xarm_model_config(6)], - visualization={"backend": "meshcat"}, - ), -) - -# XArm7 mock sim + keyboard teleop + Drake visualization -keyboard_teleop_xarm7 = autoconnect( - KeyboardTeleopModule.blueprint( - model_path=XARM7_FK_MODEL, - ee_joint_id=7, - joint_names=_xarm7_hw.joints, - ), - ControlCoordinator.blueprint( - tick_rate=100.0, - publish_joint_state=True, - joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], - tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm7_hw.joints, - priority=10, - params={"model_path": XARM7_FK_MODEL, "ee_joint_id": 7}, - ), - ], - ), - ManipulationModule.blueprint( - robots=[_xarm_model_config(7)], - visualization={"backend": "meshcat"}, - ), -) diff --git a/dimos/robot/manipulators/xarm/blueprints/agentic.py b/dimos/robot/manipulators/xarm/blueprints/agentic.py new file mode 100644 index 0000000000..073ede3607 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/agentic.py @@ -0,0 +1,46 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Agentic xArm manipulation blueprints.""" + +from __future__ import annotations + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.mcp.mcp_server import McpServer +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.agent_prompts import ( + BASE_MANIPULATION_AGENT_SYSTEM_PROMPT, + MANIPULATION_AGENT_SYSTEM_PROMPT, +) +from dimos.robot.manipulators.xarm.blueprints.basic import xarm7_planner_coordinator +from dimos.robot.manipulators.xarm.blueprints.perception import xarm_perception +from dimos.robot.manipulators.xarm.blueprints.simulation import xarm_perception_sim + +xarm7_planner_coordinator_agent = autoconnect( + xarm7_planner_coordinator, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=BASE_MANIPULATION_AGENT_SYSTEM_PROMPT), +) + +xarm_perception_agent = autoconnect( + xarm_perception, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), +) + +xarm_perception_sim_agent = autoconnect( + xarm_perception_sim, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=MANIPULATION_AGENT_SYSTEM_PROMPT), +) diff --git a/dimos/robot/manipulators/xarm/blueprints/basic.py b/dimos/robot/manipulators/xarm/blueprints/basic.py new file mode 100644 index 0000000000..4f1660342e --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/basic.py @@ -0,0 +1,91 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Basic xArm coordinator and planner blueprints.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import coordinator, planner, trajectory_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.xarm.config import ( + XARM6_SIM_PATH, + XARM7_SIM_PATH, + make_xarm6_model_config, + make_xarm7_model_config, + xarm6_hardware, + xarm7_hardware, +) + +xarm6_planner_only = ManipulationModule.blueprint( + robots=[make_xarm6_model_config(name="arm")], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, +) + +dual_xarm6_planner = ManipulationModule.blueprint( + robots=[ + make_xarm6_model_config(name="left_arm", y_offset=0.5), + make_xarm6_model_config(name="right_arm", y_offset=-0.5), + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, +) + +_xarm7_hw = xarm7_hardware("arm", gripper=True, mock_without_address=True) + +xarm7_planner_coordinator = autoconnect( + planner(robots=[make_xarm7_model_config(name="arm", add_gripper=True)]), + coordinator( + hardware=[_xarm7_hw], + tasks=[trajectory_task(_xarm7_hw)], + ), +) + +_coordinator_xarm7_hw = xarm7_hardware("arm") + +coordinator_xarm7 = autoconnect( + coordinator( + hardware=[_coordinator_xarm7_hw], + tasks=[trajectory_task(_coordinator_xarm7_hw)], + ), + *mujoco_if_sim(XARM7_SIM_PATH, len(_coordinator_xarm7_hw.joints)), +) + +_coordinator_xarm6_hw = xarm6_hardware("arm", gripper=True) + +coordinator_xarm6 = autoconnect( + coordinator( + hardware=[_coordinator_xarm6_hw], + tasks=[trajectory_task(_coordinator_xarm6_hw)], + ), + *mujoco_if_sim(XARM6_SIM_PATH, len(_coordinator_xarm6_hw.joints)), +) + +_xarm7_left = xarm7_hardware("left_arm") +_xarm6_right = xarm6_hardware("right_arm") + +coordinator_dual_xarm = ControlCoordinator.blueprint( + hardware=[_xarm7_left, _xarm6_right], + tasks=[ + TaskConfig( + name="traj_left", type="trajectory", joint_names=_xarm7_left.joints, priority=10 + ), + TaskConfig( + name="traj_right", type="trajectory", joint_names=_xarm6_right.joints, priority=10 + ), + ], +) diff --git a/dimos/robot/manipulators/xarm/blueprints/perception.py b/dimos/robot/manipulators/xarm/blueprints/perception.py new file mode 100644 index 0000000000..d0ff469251 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/perception.py @@ -0,0 +1,61 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Real-hardware xArm perception manipulation blueprints.""" + +from __future__ import annotations + +import math + +from dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera +from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule +from dimos.robot.manipulators.xarm.config import make_xarm7_model_config + +XARM_PERCEPTION_CAMERA_TRANSFORM = Transform( + translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), + rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw +) + +xarm_perception = autoconnect( + PickAndPlaceModule.blueprint( + robots=[ + make_xarm7_model_config( + name="arm", + add_gripper=True, + pitch=math.radians(45), + tf_extra_links=["link7"], + ) + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, + floor_z=-0.02, + ), + RealSenseCamera.blueprint( + base_frame_id="link7", + base_transform=XARM_PERCEPTION_CAMERA_TRANSFORM, + ), + ObjectSceneRegistrationModule.blueprint( + target_frame="world", + distance_threshold=0.08, + min_detections_for_permanent=3, + max_distance=1.0, + use_aabb=True, + max_obstacle_width=0.06, + ), +).global_config(n_workers=4) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py new file mode 100644 index 0000000000..d4c397f48b --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -0,0 +1,72 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Simulation xArm perception manipulation blueprints.""" + +from __future__ import annotations + +import math + +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.pick_and_place_module import PickAndPlaceModule +from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule +from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task +from dimos.robot.manipulators.xarm.config import ( + XARM7_SIM_PATH, + make_xarm7_model_config, + make_xarm_hardware, +) +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.visualization.rerun.bridge import RerunBridgeModule + +XARM7_SIM_HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] + +_xarm7_sim_hw = make_xarm_hardware( + "arm", + 7, + adapter_type="sim_mujoco", + address=str(XARM7_SIM_PATH), + gripper=True, + home_joints=XARM7_SIM_HOME, +) + +xarm_perception_sim = autoconnect( + PickAndPlaceModule.blueprint( + robots=[ + make_xarm7_model_config( + name="arm", + add_gripper=True, + pitch=math.radians(45), + tf_extra_links=["link7"], + home_joints=XARM7_SIM_HOME, + pre_grasp_offset=0.05, + ) + ], + planning_timeout=10.0, + visualization={"backend": "meshcat"}, + ), + MujocoSimModule.blueprint( + address=str(XARM7_SIM_PATH), + headless=False, + dof=7, + camera_name="wrist_camera", + base_frame_id="link7", + ), + ObjectSceneRegistrationModule.blueprint(target_frame="world"), + coordinator( + hardware=[_xarm7_sim_hw], + tasks=[trajectory_task(_xarm7_sim_hw)], + ), + RerunBridgeModule.blueprint(), +) diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py new file mode 100644 index 0000000000..e74c64001e --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -0,0 +1,183 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Keyboard teleop blueprints for xArm6 and xArm7.""" + +from __future__ import annotations + +from dimos.control.components import make_gripper_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task +from dimos.robot.manipulators.common.sim import mujoco_if_sim +from dimos.robot.manipulators.xarm.config import ( + XARM6_FK_MODEL, + XARM6_SIM_PATH, + XARM7_FK_MODEL, + XARM7_SIM_PATH, + make_xarm6_model_config, + make_xarm7_model_config, + make_xarm_hardware, + xarm6_hardware, + xarm7_hardware, +) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + +_xarm6_hw = make_xarm_hardware( + "arm", + 6, + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip, +) +_xarm7_hw = make_xarm_hardware( + "arm", + 7, + adapter_type="xarm" if global_config.xarm7_ip else "mock", + address=global_config.xarm7_ip, +) + +keyboard_teleop_xarm6 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=XARM6_FK_MODEL, + ee_joint_id=6, + joint_names=_xarm6_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_xarm6_hw], + tasks=[cartesian_ik_task(_xarm6_hw, model_path=XARM6_FK_MODEL, ee_joint_id=6)], + ), + ManipulationModule.blueprint( + robots=[make_xarm6_model_config(add_gripper=False)], + visualization={"backend": "meshcat"}, + ), +) + +keyboard_teleop_xarm7 = autoconnect( + KeyboardTeleopModule.blueprint( + model_path=XARM7_FK_MODEL, + ee_joint_id=7, + joint_names=_xarm7_hw.joints, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_xarm7_hw], + tasks=[cartesian_ik_task(_xarm7_hw, model_path=XARM7_FK_MODEL, ee_joint_id=7)], + ), + ManipulationModule.blueprint( + robots=[make_xarm7_model_config(add_gripper=False)], + visualization={"backend": "meshcat"}, + ), +) + +_xarm6_control_hw = make_xarm_hardware( + "arm", + 6, + adapter_type="xarm", + address=global_config.xarm6_ip, + gripper=True, +) + +coordinator_servo_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +coordinator_velocity_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +coordinator_combined_xarm6 = ControlCoordinator.blueprint( + hardware=[_xarm6_control_hw], + tasks=[ + TaskConfig( + name="servo_arm", + type="servo", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + TaskConfig( + name="velocity_arm", + type="velocity", + joint_names=_xarm6_control_hw.joints, + priority=10, + ), + ], +) + +_xarm7_teleop_hw = xarm7_hardware("arm", gripper=True) +_xarm6_teleop_hw = xarm6_hardware("arm", gripper=True) + +coordinator_teleop_xarm7 = autoconnect( + ControlCoordinator.blueprint( + hardware=[_xarm7_teleop_hw], + tasks=[ + teleop_ik_task( + _xarm7_teleop_hw, + model_path=XARM7_FK_MODEL, + ee_joint_id=7, + hand="right", + name="teleop_xarm", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.85, + "gripper_closed_pos": 0.0, + }, + ), + ], + ), + *mujoco_if_sim(XARM7_SIM_PATH, len(_xarm7_teleop_hw.joints)), +) + +coordinator_teleop_xarm6 = autoconnect( + ControlCoordinator.blueprint( + hardware=[_xarm6_teleop_hw], + tasks=[ + teleop_ik_task( + _xarm6_teleop_hw, + model_path=XARM6_FK_MODEL, + ee_joint_id=6, + hand="right", + name="teleop_xarm", + params={ + "gripper_joint": make_gripper_joints("arm")[0], + "gripper_open_pos": 0.85, + "gripper_closed_pos": 0.0, + }, + ), + ], + ), + *mujoco_if_sim(XARM6_SIM_PATH, len(_xarm6_teleop_hw.joints)), +) diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py new file mode 100644 index 0000000000..d918ee94a8 --- /dev/null +++ b/dimos/robot/manipulators/xarm/config.py @@ -0,0 +1,208 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""xArm planning model configuration helpers.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import ( + base_pose, + coordinator_joint_mapping, + joint_names, +) +from dimos.utils.data import LfsPath + +XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("right_inner_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "left_outer_knuckle"), + ("right_inner_knuckle", "right_finger"), + ("left_inner_knuckle", "left_finger"), + ("left_finger", "right_finger"), + ("left_outer_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "right_inner_knuckle"), + ("left_outer_knuckle", "right_finger"), + ("right_outer_knuckle", "left_finger"), + ("xarm_gripper_base_link", "left_inner_knuckle"), + ("xarm_gripper_base_link", "right_inner_knuckle"), + ("xarm_gripper_base_link", "left_finger"), + ("xarm_gripper_base_link", "right_finger"), + ("link6", "xarm_gripper_base_link"), + ("link6", "left_outer_knuckle"), + ("link6", "right_outer_knuckle"), +] + +XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" +XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} +XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") +XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") +XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") +XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") + + +def _adapter_kwargs(home_joints: list[float] | None = None) -> dict[str, object]: + if home_joints is None: + return {} + return {"initial_positions": home_joints} + + +def make_xarm_hardware( + hw_id: str, + dof: int, + *, + adapter_type: str = "mock", + address: str | None = None, + gripper: bool = False, + auto_enable: bool = True, + adapter_kwargs: dict[str, object] | None = None, + home_joints: list[float] | None = None, +) -> HardwareComponent: + kwargs = _adapter_kwargs(home_joints) + if adapter_kwargs: + kwargs.update(adapter_kwargs) + return HardwareComponent( + hardware_id=hw_id, + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints(hw_id, dof), + adapter_type=adapter_type, + address=address, + auto_enable=auto_enable, + gripper_joints=[f"{hw_id}/gripper"] if gripper else [], + adapter_kwargs=kwargs, + ) + + +def xarm7_hardware( + hw_id: str = "arm", + *, + gripper: bool = False, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_xarm_hardware( + hw_id, + 7, + adapter_type="sim_mujoco", + address=str(XARM7_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.xarm7_ip + if mock_without_address and not address: + return make_xarm_hardware(hw_id, 7, gripper=gripper, home_joints=home_joints) + return make_xarm_hardware( + hw_id, + 7, + adapter_type="xarm", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def xarm6_hardware( + hw_id: str = "arm", + *, + gripper: bool = False, + mock_without_address: bool = False, + home_joints: list[float] | None = None, +) -> HardwareComponent: + if global_config.simulation: + return make_xarm_hardware( + hw_id, + 6, + adapter_type="sim_mujoco", + address=str(XARM6_SIM_PATH), + gripper=gripper, + home_joints=home_joints, + ) + address = global_config.xarm6_ip + if mock_without_address and not address: + return make_xarm_hardware(hw_id, 6, gripper=gripper, home_joints=home_joints) + return make_xarm_hardware( + hw_id, + 6, + adapter_type="xarm", + address=address, + gripper=gripper, + home_joints=home_joints, + ) + + +def make_xarm_model_config( + name: str, + dof: int, + *, + add_gripper: bool = True, + x_offset: float = 0.0, + y_offset: float = 0.0, + z_offset: float = 0.0, + pitch: float = 0.0, + joint_prefix: str | None = None, + coordinator_task_name: str | None = None, + tf_extra_links: list[str] | None = None, + home_joints: list[float] | None = None, + pre_grasp_offset: float = 0.10, +) -> RobotModelConfig: + xacro_args = { + "dof": str(dof), + "limited": "true", + "attach_xyz": f"{x_offset} {y_offset} {z_offset}", + "attach_rpy": f"0 {pitch} 0", + } + if add_gripper: + xacro_args["add_gripper"] = "true" + + return RobotModelConfig( + name=name, + model_path=XARM_MODEL_PATH, + base_pose=base_pose(x_offset, y_offset, z_offset), + joint_names=joint_names(dof), + end_effector_link="link_tcp" if add_gripper else f"link{dof}", + base_link="link_base", + package_paths=XARM_PACKAGE_PATHS, + xacro_args=xacro_args, + auto_convert_meshes=True, + collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), + joint_name_mapping=coordinator_joint_mapping( + name, + dof, + joint_prefix=joint_prefix, + ), + coordinator_task_name=coordinator_task_name or f"traj_{name}", + gripper_hardware_id=name if add_gripper else None, + tf_extra_links=tf_extra_links or [], + home_joints=home_joints or [0.0] * dof, + pre_grasp_offset=pre_grasp_offset, + ) + + +def make_xarm6_model_config( + name: str = "arm", + **kwargs: Any, +) -> RobotModelConfig: + return make_xarm_model_config(name, 6, **kwargs) + + +def make_xarm7_model_config( + name: str = "arm", + **kwargs: Any, +) -> RobotModelConfig: + return make_xarm_model_config(name, 7, **kwargs) diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index 24c4bfc2e1..0b6644960a 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -20,17 +20,17 @@ """ from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE -from dimos.control.blueprints.teleop import ( - coordinator_teleop_dual, - coordinator_teleop_piper, - coordinator_teleop_xarm6, - coordinator_teleop_xarm7, -) from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport, pSHMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.manipulators.common.mixed import coordinator_teleop_dual +from dimos.robot.manipulators.piper.blueprints.teleop import coordinator_teleop_piper +from dimos.robot.manipulators.xarm.blueprints.teleop import ( + coordinator_teleop_xarm6, + coordinator_teleop_xarm7, +) from dimos.robot.unitree.go2.connection import GO2Connection from dimos.teleop.quest.quest_extensions import ( ArmTeleopModule, diff --git a/dimos/teleop/quest_hosted/blueprints.py b/dimos/teleop/quest_hosted/blueprints.py index 199ae0c3ba..5a182f9118 100644 --- a/dimos/teleop/quest_hosted/blueprints.py +++ b/dimos/teleop/quest_hosted/blueprints.py @@ -17,8 +17,8 @@ from pathlib import Path from dimos.constants import STATE_DIR -from dimos.control.blueprints.teleop import coordinator_teleop_xarm7 from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.xarm.blueprints.teleop import coordinator_teleop_xarm7 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic from dimos.teleop.quest_hosted.hosted_extensions import ( HostedArmTeleopModule, diff --git a/docs/capabilities/manipulation/a750.md b/docs/capabilities/manipulation/a750.md index 8f4402ead0..ea1117eb6e 100644 --- a/docs/capabilities/manipulation/a750.md +++ b/docs/capabilities/manipulation/a750.md @@ -49,8 +49,10 @@ DEVICE_PATH=/dev/ttyACM0 dimos run keyboard-teleop-a750 ## Robot Model -The robot model, hardware, and task config used by the runnable stack are -defined in [`dimos/robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py). +The robot model and hardware config are defined in +[`dimos/robot/manipulators/a750/config.py`](/dimos/robot/manipulators/a750/config.py). +The runnable keyboard teleop stack is composed in +[`dimos/robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py). | Field | Value | |-------|-------| @@ -108,7 +110,8 @@ If `a750_control` is not installed, `connect()` records an error and returns `Fa ## Control Path -The `keyboard-teleop-a750` blueprint is defined in [`dimos/robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py#L22). +The `keyboard-teleop-a750` blueprint is defined in +[`dimos/robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py). ```text KeyboardTeleopModule diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index f5ca1c536f..aaa075ee49 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -630,7 +630,7 @@ def test_write_positions(mock_adapter): ### Integration test with coordinator ```python skip -from dimos.control.blueprints.basic import coordinator_mock +from dimos.robot.manipulators.common.mock import coordinator_mock from dimos.core.coordination.module_coordinator import ModuleCoordinator # Build and start coordinator with mock hardware diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index b70d9e9f55..45670e5445 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -218,7 +218,7 @@ If you don't know which Cartesian targets are reachable, check first with the wo ### Which CAN bus is which arm -Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants at the top of [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py): +Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants in [config.py](/dimos/robot/manipulators/openarm/config.py): ```python LEFT_CAN = "can0" @@ -247,7 +247,7 @@ The URDFs use the xacro-generated limits (which include per-side offsets for mir ### Disabling auto MIT-mode write -The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py) and restart — the arms should still respond. +The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [config.py](/dimos/robot/manipulators/openarm/config.py) and restart — the arms should still respond. --- @@ -335,10 +335,10 @@ Persistent across power cycles. - **`ip link ... fd on` → `Operation not supported`.** gs_usb firmware doesn't support CAN-FD. Use classical CAN @ 1 Mbit (our bringup script's default). - **Motors reply to probes but commands do nothing.** CTRL_MODE is not MIT. The adapter now writes MIT on connect, but if you disabled that and motors got reset, run `openarm_set_mit_mode.py`. -- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm blueprint module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. +- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm config module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. - **`INVALID_START` during planning.** Hardware encoder noise pushed a joint 1 mrad past a URDF limit. Joint4 used to be exactly `lower=0.0` which tripped this — it's now `-0.01` to give breathing room. If you see it on a different joint, widen that limit by ~10 mrad. - **"Transmit buffer full" (ENOBUFS) at 100 Hz.** Kernel TX queue too small. The bringup script sets `txqueuelen 1000`; the driver also retries on ENOBUFS. If you still see the error, check `ip -details link show canX | grep qlen`. -- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py). +- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [config.py](/dimos/robot/manipulators/openarm/config.py). --- diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index d991cb3f21..e502947699 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -199,10 +199,13 @@ visualization backend. | File | Description | |------|-------------| | [`manipulation_module.py`](/dimos/manipulation/manipulation_module.py) | Main module (RPC interface, state machine) | -| [`manipulation/blueprints.py`](/dimos/manipulation/blueprints.py) | Planner and perception blueprints | -| [`robot/manipulators/a750/blueprints.py`](/dimos/robot/manipulators/a750/blueprints.py) | A-750 keyboard teleop blueprint | -| [`robot/manipulators/piper/blueprints.py`](/dimos/robot/manipulators/piper/blueprints.py) | Piper keyboard teleop blueprint | -| [`robot/manipulators/xarm/blueprints.py`](/dimos/robot/manipulators/xarm/blueprints.py) | XArm keyboard teleop blueprints | +| [`robot/manipulators/common/blueprints.py`](/dimos/robot/manipulators/common/blueprints.py) | Shared coordinator, planner, and task helpers | +| [`robot/manipulators/a750/config.py`](/dimos/robot/manipulators/a750/config.py) | A-750 model and hardware config | +| [`robot/manipulators/a750/blueprints/teleop.py`](/dimos/robot/manipulators/a750/blueprints/teleop.py) | A-750 keyboard teleop blueprint | +| [`robot/manipulators/piper/blueprints/basic.py`](/dimos/robot/manipulators/piper/blueprints/basic.py) | Piper coordinator blueprint | +| [`robot/manipulators/piper/blueprints/teleop.py`](/dimos/robot/manipulators/piper/blueprints/teleop.py) | Piper teleop blueprints | +| [`robot/manipulators/xarm/blueprints/basic.py`](/dimos/robot/manipulators/xarm/blueprints/basic.py) | XArm coordinator and planner blueprints | +| [`robot/manipulators/xarm/blueprints/perception.py`](/dimos/robot/manipulators/xarm/blueprints/perception.py) | XArm perception blueprint | | [`teleop/keyboard/keyboard_teleop_module.py`](/dimos/teleop/keyboard/keyboard_teleop_module.py) | Keyboard teleop module | | [`planning/world/drake_world.py`](/dimos/manipulation/planning/world/drake_world.py) | Drake physics backend | | [`planning/planners/rrt_planner.py`](/dimos/manipulation/planning/planners/rrt_planner.py) | RRT-Connect motion planner | From 29f52af53524dc26a139a268b36f3ffdd58f87c5 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 10:21:04 -0700 Subject: [PATCH 2/8] fix: address review and ci failures --- dimos/e2e_tests/test_control_coordinator.py | 23 +++++++++---------- dimos/manipulation/manipulation_module.py | 6 +++-- .../planning/world/drake_world.py | 7 +----- dimos/manipulation/test_manipulation_unit.py | 15 ++++++++++++ dimos/robot/test_all_blueprints.py | 3 +-- 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 8d635805b8..40e4800b47 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -201,8 +201,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: """Test dual-arm coordinator with independent trajectories.""" lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") - # Start integrated dual-arm mock planner/coordinator - start_blueprint("dual-xarm6-planner-coordinator") + # Start dual-arm mock coordinator + start_blueprint("coordinator-dual-mock") lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") client = RPCClient(None, ControlCoordinator) @@ -213,15 +213,15 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: assert "right_arm/joint1" in joints tasks = client.list_tasks() - assert "traj_left_arm" in tasks - assert "traj_right_arm" in tasks + assert "traj_left" in tasks + assert "traj_right" in tasks # Create trajectories for both arms left_trajectory = JointTrajectory( - joint_names=[f"left_arm/joint{i + 1}" for i in range(6)], + joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 6), + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), + TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), ], ) @@ -235,11 +235,10 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: # Execute both via task_invoke assert ( - client.task_invoke("traj_left_arm", "execute", {"trajectory": left_trajectory}) - is True + client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True ) assert ( - client.task_invoke("traj_right_arm", "execute", {"trajectory": right_trajectory}) + client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory}) is True ) @@ -247,8 +246,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: time.sleep(1.0) # Both should complete - left_state = client.task_invoke("traj_left_arm", "get_state") - right_state = client.task_invoke("traj_right_arm", "get_state") + left_state = client.task_invoke("traj_left", "get_state") + right_state = client.task_invoke("traj_right", "get_state") assert left_state == TrajectoryState.COMPLETED assert right_state == TrajectoryState.COMPLETED diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 6e67bd95e4..acdc677f1d 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -852,7 +852,8 @@ def plan_to_pose_targets( if self._world_monitor is None or self._kinematics is None: return False if not pose_targets: - return self._fail("At least one pose target is required") + logger.error("At least one pose target is required") + return False stamped_targets = { planning_group_id_from_selector(group): PoseStamped( @@ -909,7 +910,8 @@ def plan_to_joint_targets( if self._world_monitor is None or self._planner is None: return False if not joint_targets: - return self._fail("At least one joint target is required") + logger.error("At least one joint target is required") + return False group_ids = tuple( dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index daa0a25158..b639674c4c 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -893,11 +893,6 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] - if robot_data.ee_frame is None: - raise ValueError( - f"Robot '{robot_id}' has no robot-scoped end-effector link; " - "use get_group_ee_pose() with an explicit planning group ID" - ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) full_positions = self._plant.GetPositions(plant_ctx) @@ -1018,7 +1013,7 @@ def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: if robot_data.ee_frame is None: raise ValueError( f"Robot '{robot_id}' has no robot-scoped end-effector link; " - "use get_group_jacobian() with an explicit planning group ID" + "use get_group_ee_pose() with an explicit planning group ID" ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 833feb029b..876fcdc8cd 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -178,6 +178,21 @@ def test_begin_planning_state_checks(self, robot_config): module._state = ManipulationState.EXECUTING assert module._begin_planning() is False + def test_empty_plan_targets_do_not_fault_before_planning(self): + """Pre-flight validation errors do not corrupt the planning state machine.""" + module = _make_module() + module._world_monitor = MagicMock() + module._kinematics = MagicMock() + module._planner = MagicMock() + + assert module.plan_to_pose_targets({}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + + assert module.plan_to_joint_targets({}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + class TestRobotSelection: """Test robot selection logic.""" diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index b0929ff1b6..cdf72e9b6b 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -49,8 +49,7 @@ "coordinator-velocity-xarm6", "coordinator-xarm6", "coordinator-xarm7", - "dual-xarm6-planner-coordinator", - "dual-xarm7-planner-coordinator", + "dual-xarm6-planner", "teleop-hosted-go2", "teleop-hosted-xarm7", "teleop-quest-dual", From ded5457883792517bd2690fd4e78236f2fd9f97c Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 10:56:49 -0700 Subject: [PATCH 3/8] fix: guard pose planning without planner --- dimos/manipulation/manipulation_module.py | 2 +- dimos/manipulation/test_manipulation_unit.py | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index acdc677f1d..908503ebca 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -849,7 +849,7 @@ def plan_to_pose_targets( auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), ) -> bool: """Plan to one or more group pose targets with optional auxiliary groups.""" - if self._world_monitor is None or self._kinematics is None: + if self._world_monitor is None or self._kinematics is None or self._planner is None: return False if not pose_targets: logger.error("At least one pose target is required") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 876fcdc8cd..33ebb4d267 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -193,6 +193,19 @@ def test_empty_plan_targets_do_not_fault_before_planning(self): assert module._state == ManipulationState.IDLE assert module._error_message == "" + def test_plan_to_pose_targets_requires_planner_before_planning(self): + """Pose-target planning requires both IK and path planner backends.""" + module = _make_module() + module._world_monitor = MagicMock() + module._kinematics = MagicMock() + module._planner = None + + pose = Pose(position=Vector3(), orientation=Quaternion()) + + assert module.plan_to_pose_targets({"test_arm/manipulator": pose}) is False + assert module._state == ManipulationState.IDLE + assert module._error_message == "" + class TestRobotSelection: """Test robot selection logic.""" From 06da1fd7113a06660208e998d4cff7758be0c184 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 11:02:02 -0700 Subject: [PATCH 4/8] refactor: remove planning identifiers shim --- .../planning/planning_identifiers.py | 31 ------------------- 1 file changed, 31 deletions(-) delete mode 100644 dimos/manipulation/planning/planning_identifiers.py diff --git a/dimos/manipulation/planning/planning_identifiers.py b/dimos/manipulation/planning/planning_identifiers.py deleted file mode 100644 index 1acd15e824..0000000000 --- a/dimos/manipulation/planning/planning_identifiers.py +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Compatibility shim for planning-group identifier helpers. - -New code should import from ``dimos.manipulation.planning.groups.identifiers``. -""" - -from dimos.manipulation.planning.groups.identifiers import ( # noqa: F401 - assert_global_joint_names, - assert_local_joint_names, - assert_valid_local_joint_name, - assert_valid_robot_name, - is_global_joint_name, - local_joint_name_from_global, - make_global_joint_name, - make_global_joint_names, - make_planning_group_id, - parse_planning_group_id, -) From 0478556eb572cb4030f215166f274a8eb1ecc97f Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 11:28:05 -0700 Subject: [PATCH 5/8] chore: group util->joints --- dimos/manipulation/manipulation_module.py | 4 +- .../planning/groups/identifiers.py | 35 ++++++++++++----- .../planning/groups/{utils.py => joints.py} | 2 +- .../planning/kinematics/jacobian_ik.py | 2 +- .../planning/kinematics/pink_ik.py | 2 +- .../test_planning_group_identifiers.py | 39 +++++++++++++++++++ ...utils.py => test_planning_group_joints.py} | 2 +- 7 files changed, 71 insertions(+), 15 deletions(-) rename dimos/manipulation/planning/groups/{utils.py => joints.py} (98%) create mode 100644 dimos/manipulation/planning/test_planning_group_identifiers.py rename dimos/manipulation/planning/{test_planning_group_utils.py => test_planning_group_joints.py} (96%) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 908503ebca..86d63f5ade 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -45,12 +45,12 @@ assert_local_joint_names, make_global_joint_names, ) -from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.groups.utils import ( +from dimos.manipulation.planning.groups.joints import ( filter_joint_state_to_selected_joints, joint_target_to_global_names, planning_group_id_from_selector, ) +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.kinematics.config import ( ManipulationKinematicsConfig, PinkKinematicsConfig, diff --git a/dimos/manipulation/planning/groups/identifiers.py b/dimos/manipulation/planning/groups/identifiers.py index db9d990081..7ce0fc0774 100644 --- a/dimos/manipulation/planning/groups/identifiers.py +++ b/dimos/manipulation/planning/groups/identifiers.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Planning-group and global-joint identifier helpers.""" +"""String grammar helpers for public manipulation identifiers.""" from __future__ import annotations @@ -74,7 +74,7 @@ def make_global_joint_name( def make_global_joint_names( robot_name: RobotName, - local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], + local_joint_names: Sequence[LocalModelJointName], ) -> list[GlobalJointName]: """Convert local model joint names to public global joint names.""" return [make_global_joint_name(robot_name, name) for name in local_joint_names] @@ -86,6 +86,28 @@ def is_global_joint_name(name: str) -> bool: return len(parts) == 2 and bool(parts[0]) and bool(parts[1]) +def parse_global_joint_name( + global_joint_name: GlobalJointName, +) -> tuple[RobotName, LocalModelJointName]: + """Split and validate a global joint name.""" + parts = global_joint_name.split("/", maxsplit=1) + if len(parts) != 2: + raise ValueError( + f"Invalid global joint name {global_joint_name!r}; " + "expected '{robot_name}/{local_joint_name}'" + ) + robot_name, local_name = parts + try: + assert_valid_robot_name(robot_name) + assert_valid_local_joint_name(local_name) + except ValueError as exc: + raise ValueError( + f"Invalid global joint name {global_joint_name!r}; " + "expected '{robot_name}/{local_joint_name}'" + ) from exc + return robot_name, local_name + + def assert_global_joint_names(names: Sequence[GlobalJointName]) -> None: """Validate that names are global joint names.""" invalid = [name for name in names if not is_global_joint_name(name)] @@ -99,14 +121,9 @@ def local_joint_name_from_global( ) -> LocalModelJointName: """Validate and strip a global joint name for backend internals.""" assert_valid_robot_name(robot_name) - prefix = f"{robot_name}/" - if not global_joint_name.startswith(prefix): + parsed_robot_name, local_name = parse_global_joint_name(global_joint_name) + if parsed_robot_name != robot_name: raise ValueError( f"Global joint name {global_joint_name!r} does not belong to robot {robot_name!r}" ) - local_name = global_joint_name[len(prefix) :] - try: - assert_valid_local_joint_name(local_name) - except ValueError as exc: - raise ValueError(f"Invalid global joint name: {global_joint_name!r}") from exc return local_name diff --git a/dimos/manipulation/planning/groups/utils.py b/dimos/manipulation/planning/groups/joints.py similarity index 98% rename from dimos/manipulation/planning/groups/utils.py rename to dimos/manipulation/planning/groups/joints.py index e55f997a33..c66b4410d6 100644 --- a/dimos/manipulation/planning/groups/utils.py +++ b/dimos/manipulation/planning/groups/joints.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Shared helpers for planning-group selectors and joint-state projection.""" +"""Planning-group joint target and joint-state helpers.""" from collections.abc import Mapping, Sequence diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 9988b2009c..e250fd26d1 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -30,8 +30,8 @@ import numpy as np +from dimos.manipulation.planning.groups.joints import filter_joint_state_to_selected_joints from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection -from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index ef46cad981..f5169d140b 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -24,9 +24,9 @@ import numpy as np from dimos.manipulation.planning.groups.identifiers import make_global_joint_name +from dimos.manipulation.planning.groups.joints import matching_global_joint_name from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry -from dimos.manipulation.planning.groups.utils import matching_global_joint_name from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus diff --git a/dimos/manipulation/planning/test_planning_group_identifiers.py b/dimos/manipulation/planning/test_planning_group_identifiers.py new file mode 100644 index 0000000000..d728123e8f --- /dev/null +++ b/dimos/manipulation/planning/test_planning_group_identifiers.py @@ -0,0 +1,39 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for planning group identifier grammar.""" + +from __future__ import annotations + +import pytest + +from dimos.manipulation.planning.groups.identifiers import ( + local_joint_name_from_global, + parse_global_joint_name, +) + + +def test_parse_global_joint_name_returns_robot_and_local_joint() -> None: + assert parse_global_joint_name("left_arm/joint1") == ("left_arm", "joint1") + + +@pytest.mark.parametrize("name", ["joint1", "/joint1", "left_arm/", "left_arm/foo/bar"]) +def test_parse_global_joint_name_rejects_invalid_names(name: str) -> None: + with pytest.raises(ValueError, match="Invalid global joint name"): + parse_global_joint_name(name) + + +def test_local_joint_name_from_global_requires_matching_robot() -> None: + with pytest.raises(ValueError, match="does not belong to robot"): + local_joint_name_from_global("right_arm", "left_arm/joint1") diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_joints.py similarity index 96% rename from dimos/manipulation/planning/test_planning_group_utils.py rename to dimos/manipulation/planning/test_planning_group_joints.py index d479f34a4e..0391b1b136 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_joints.py @@ -18,8 +18,8 @@ import pytest +from dimos.manipulation.planning.groups.joints import joint_target_to_global_names from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.groups.utils import joint_target_to_global_names from dimos.msgs.sensor_msgs.JointState import JointState From 5c573f450910cf12520baed76daac9b515350568 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 12:09:59 -0700 Subject: [PATCH 6/8] doc: update planning group doc --- .../manipulation/planning_groups.md | 103 +++++++++++++++--- 1 file changed, 85 insertions(+), 18 deletions(-) diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md index 24343b5bc1..7b86182778 100644 --- a/docs/capabilities/manipulation/planning_groups.md +++ b/docs/capabilities/manipulation/planning_groups.md @@ -11,12 +11,24 @@ being planned. | Planning group | A named serial chain of controllable robot joints. | | Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | | Global joint name | Boundary-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Local joint name | The joint name as it appears in the robot model. | | Generated plan | Minimal planning artifact containing selected group IDs and one synchronized global-joint path. | | Auxiliary group | A group selected for a pose request without receiving its own pose target. | Local URDF/SRDF joint names stay inside robot-scoped APIs, model parsing, and -backend internals. Flat planning states and generated plan paths require global -joint names so two robots can safely have the same local joint names. +backend internals. Group-scoped APIs, generated plans, preview, execution, and +coordinator boundaries use global joint names so two robots can safely have the +same local joint names. + +`PlanningGroup` descriptors returned by `list_planning_groups()` include both +namespaces: + +- `id`: public `{robot_name}/{group_name}` selector; +- `joint_names`: selected global joint names in group order; +- `local_joint_names`: selected local model joint names in group order; +- `base_link` / `tip_link`: model links for group kinematics. `tip_link=None` + means the group can participate as an auxiliary group but cannot receive a + pose target. ## Planning group sources @@ -64,20 +76,33 @@ name is always `manipulator`. ## Planning APIs Planning APIs select groups explicitly. Descriptors returned by -`WorldSpec.list_planning_groups()` can be passed where a group ID is accepted; -the API normalizes them back to IDs and re-resolves current world state. +`ManipulationModule.list_planning_groups()` can be passed anywhere a group ID is +accepted; the module normalizes descriptors back to IDs and re-resolves current +world state. ```python skip -# Joint-space planning for one group. +# Discover groups. Each item is a PlanningGroup dataclass. +groups = manip.list_planning_groups() +arm = groups[0] + +# Joint-space planning for one group. Named targets may use global names... manip.plan_to_joint_targets({ - "left_arm/manipulator": JointState( + arm.id: JointState( + name=["left_arm/joint1", "left_arm/joint2"], + position=[0.2, -0.1], + ) +}) + +# ...or local model names, but not a mix of both namespaces. +manip.plan_to_joint_targets({ + arm: JointState( name=["joint1", "joint2"], position=[0.2, -0.1], ) }) # Pose planning for an arm while a torso/waist group participates as free DOFs. -manip.plan_to_poses( +manip.plan_to_pose_targets( {"robot/arm": target_pose}, auxiliary_groups=["robot/torso"], ) @@ -87,24 +112,63 @@ manip.preview_plan(plan) manip.execute_plan(plan) ``` -For robot-scoped joint-space planning, unnamed vectors are interpreted in robot -model joint order. If names are provided, they must be local model joint names: -no global names, missing joints, extra joints, or partial joint sets. +### Pose targets + +`plan_to_pose_targets()` accepts `Mapping[PlanningGroupID | PlanningGroup, +Pose]`. It wraps each `Pose` in the world frame before calling the group-scoped +IK path. Every key in `pose_targets` must refer to a pose-targetable group +(`tip_link` is not `None`). `auxiliary_groups` may include non-pose-targeted +groups whose joints should stay in the solve as free DOFs. + +`inverse_kinematics()` is the lower-level RPC. It accepts stamped poses keyed by +group ID plus optional auxiliary group IDs and returns an `IKResult` without +running collision filtering or planning. + +The compatibility wrapper `plan_to_pose(pose, robot_name=None)` still exists. It +selects the default pose-targetable group for the robot, then delegates to +`plan_to_pose_targets()`. + +### Joint targets + +`plan_to_joint_targets()` accepts `Mapping[PlanningGroupID | PlanningGroup, +JointState]`. + +For a group-scoped joint target: + +- an unnamed vector is interpreted in that group's joint order; +- named targets may use all-global names or all-local names; +- global and local names must not be mixed in one target; +- named targets must provide exactly the group's selected joints, with no + missing or extra joints. + +The compatibility wrapper `plan_to_joints(joints, robot_name=None)` still exists. +It selects the robot's default group, then delegates to +`plan_to_joint_targets()`. + +Robot-scoped state helpers such as `set_init_joints()` still use local model +joint names. When unnamed, those vectors are interpreted in full robot model +joint order. ## Generated plans and execution A `GeneratedPlan` stores: - selected planning group IDs; -- a single synchronized path of `JointState` waypoints keyed by global joint - names; +- a single synchronized path of `JointState` waypoints keyed by selected global + joint names; - status, timing, path length, iteration count, and message metadata. -Preview and execution project this path lazily. Preview sends projected joint -paths to the world monitor. Execution splits the path by affected trajectory -task, orders each trajectory by the robot's configured local joint order, writes -global joint names at the coordinator boundary, and invokes each trajectory -controller. Controllers remain planning-group agnostic. +Preview and execution project this path lazily. `preview_plan(plan=None, +duration=None, robot_name=None)` defaults to `_last_plan`; `robot_name` is only a +filter that rejects plans which do not affect the requested robot. Preview sends +the generated global-joint path to the world monitor for animation. + +`execute_plan(plan=None)` also defaults to `_last_plan`. It infers affected +robots from the selected groups, projects each waypoint back into each robot's +full local model joint order, fills unselected robot joints from current state, +then writes a coordinator `JointTrajectory` using global joint names. The +trajectory is dispatched to each robot's configured `coordinator_task_name`. +Controllers remain planning-group agnostic. Multi-task dispatch is not atomic in this change: if one trajectory task accepts and a later task rejects, DimOS reports the rejection but does not roll back the @@ -120,4 +184,7 @@ New planning logic should use model/SRDF structure and planning group base/tip links instead. Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains -supported and should describe the ordered controllable local model joint set. +supported and should describe the ordered controllable local model joint set, not +a planning group. `joint_name_mapping` can map external/coordinator joint names +back to local model joint names for adapters that publish scoped hardware names. +`coordinator_task_name` identifies the trajectory task used by `execute_plan()`. From e6f361eb84a9101c2c9d188dbdfec1de28f33dd7 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 15:07:40 -0700 Subject: [PATCH 7/8] chore: split bulky viser panel changes --- dimos/manipulation/manipulation_module.py | 1 + .../visualization/viser/adapter.py | 241 +++++ .../visualization/viser/animation.py | 33 +- dimos/manipulation/visualization/viser/gui.py | 944 +++++------------ .../visualization/viser/panel_backend.py | 268 ----- .../manipulation/visualization/viser/scene.py | 158 +-- .../manipulation/visualization/viser/state.py | 53 +- .../visualization/viser/test_gui_status.py | 44 +- .../viser/test_operation_worker.py | 35 +- .../visualization/viser/test_state.py | 3 - .../viser/test_viser_visualization.py | 948 +++++------------- .../viser/test_visualizer_lifecycle.py | 135 +-- .../manipulation/visualization/viser/theme.py | 6 +- .../visualization/viser/visualizer.py | 88 +- 14 files changed, 870 insertions(+), 2087 deletions(-) create mode 100644 dimos/manipulation/visualization/viser/adapter.py delete mode 100644 dimos/manipulation/visualization/viser/panel_backend.py diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 86d63f5ade..0bcd4c0db9 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -1028,6 +1028,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfo | Non "base_link": config.base_link, "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, + "has_joint_name_mapping": bool(config.joint_name_mapping), "coordinator_task_name": config.coordinator_task_name, "home_joints": config.home_joints, "pre_grasp_offset": config.pre_grasp_offset, diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py new file mode 100644 index 0000000000..48b84c6f55 --- /dev/null +++ b/dimos/manipulation/visualization/viser/adapter.py @@ -0,0 +1,241 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING, cast + +from dimos.manipulation.planning.spec.enums import IKStatus +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.msgs.sensor_msgs.JointState import JointState + +if TYPE_CHECKING: + from dimos.manipulation.manipulation_module import ManipulationModule + from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor + from dimos.manipulation.planning.spec.config import RobotModelConfig + from dimos.manipulation.planning.spec.models import JointPath, RobotName, WorldRobotID + from dimos.msgs.geometry_msgs.Pose import Pose + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + +def copy_joint_state(joint_state: JointState | None) -> JointState | None: + """Make a local copy of a JointState-like message for rendering.""" + return None if joint_state is None else JointState(joint_state) + + +class InProcessViserAdapter: + """Small in-process boundary between Viser callbacks and manipulation internals.""" + + def __init__( + self, + *, + world_monitor: WorldMonitor, + manipulation_module: ManipulationModule, + ) -> None: + self._world_monitor = world_monitor + self._module = manipulation_module + + def list_robots(self) -> list[RobotName]: + return list(self._module.list_robots()) + + def robot_items(self) -> list[tuple[RobotName, WorldRobotID, RobotModelConfig]]: + return self._module.robot_items() + + def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: + return self._module.robot_id_for_name(robot_name) + + def robot_name_for_id(self, robot_id: WorldRobotID) -> RobotName | None: + return self._module.robot_name_for_id(robot_id) + + def get_robot_config(self, robot_name: RobotName) -> RobotModelConfig | None: + return self._module.get_robot_config(robot_name) + + def get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: + info = self._module.get_robot_info(robot_name) + if info is None: + return None + return info + + def get_init_joints(self, robot_name: RobotName) -> JointState | None: + return copy_joint_state(self._module.get_init_joints(robot_name)) + + def get_current_joint_state(self, robot_name: RobotName) -> JointState | None: + robot_id = self.robot_id_for_name(robot_name) + if robot_id is None: + return None + return copy_joint_state(self._world_monitor.get_current_joint_state(robot_id)) + + def is_state_stale(self, robot_name: RobotName, max_age: float = 1.0) -> bool: + robot_id = self.robot_id_for_name(robot_name) + return True if robot_id is None else self._world_monitor.is_state_stale(robot_id, max_age) + + def get_ee_pose( + self, robot_name: RobotName, joint_state: JointState | None = None + ) -> PoseStamped | None: + robot_id = self.robot_id_for_name(robot_name) + if robot_id is None: + return None + return self._world_monitor.get_ee_pose(robot_id, copy_joint_state(joint_state)) + + def evaluate_joint_target(self, joints: JointState, robot_name: RobotName) -> TargetEvaluation: + """Evaluate a legacy robot-scoped joint target through group-scoped APIs.""" + legacy_evaluate = getattr(self._module, "evaluate_joint_target", None) + if callable(legacy_evaluate): + result = cast("TargetEvaluation", legacy_evaluate(JointState(joints), robot_name)) + joint_state = result.get("joint_state") + result["joint_state"] = copy_joint_state( + joint_state if isinstance(joint_state, JointState) else None + ) + return result + default_group_id = getattr(self._module, "_default_group_id_for_robot", None) + joint_target_to_global = getattr(self._module, "_joint_target_to_global_names", None) + if not callable(default_group_id) or not callable(joint_target_to_global): + robot_id = self.robot_id_for_name(robot_name) + valid = ( + False + if robot_id is None + else bool(self._world_monitor.is_state_valid(robot_id, JointState(joints))) + ) + return { + "success": valid, + "status": "FEASIBLE" if valid else "COLLISION", + "message": "Target is valid" if valid else "Target is in collision", + "collision_free": valid, + "joint_state": JointState(joints), + } + group_id = self._module._default_group_id_for_robot(robot_name) + if group_id is None: + return {"success": False, "status": "INVALID", "message": "No default group"} + target = self._module._joint_target_to_global_names(group_id, JointState(joints)) + if target is None: + return {"success": False, "status": "INVALID", "message": "Invalid joint target"} + collision = self._module.check_collision(target) + collision_free = collision.collision_free is True + return { + "success": collision_free, + "status": collision.status, + "message": collision.message, + "collision_free": collision_free, + "joint_state": copy_joint_state(target), + } + + def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: + """Evaluate a legacy robot-scoped pose target through group-scoped IK.""" + ik = self._module.inverse_kinematics_single(pose, robot_name=robot_name) + if not ik.is_success() or ik.joint_state is None: + return { + "success": False, + "status": ik.status.name, + "message": ik.message, + "collision_free": False, + "joint_state": None, + "position_error": ik.position_error, + "orientation_error": ik.orientation_error, + } + collision = self._module.check_collision(ik.joint_state) + collision_free = collision.collision_free is True + return { + "success": collision_free, + "status": collision.status if not collision_free else IKStatus.SUCCESS.name, + "message": collision.message, + "collision_free": collision_free, + "joint_state": copy_joint_state(ik.joint_state), + "position_error": ik.position_error, + "orientation_error": ik.orientation_error, + } + + def get_planned_path(self, robot_name: RobotName) -> JointPath | None: + legacy_get_planned_path = getattr(self._module, "get_planned_path", None) + if callable(legacy_get_planned_path): + legacy_path = legacy_get_planned_path(robot_name) + if legacy_path is None: + return None + if not isinstance(legacy_path, list): + return None + copied = [copy_joint_state(point) for point in legacy_path] + return [point for point in copied if point is not None] + planned_paths = getattr(self._module, "_planned_paths", None) + if isinstance(planned_paths, dict): + path_obj = planned_paths.get(robot_name) + if isinstance(path_obj, list): + copied = [copy_joint_state(point) for point in path_obj] + return [point for point in copied if point is not None] + plan = getattr(self._module, "_last_plan", None) + config = self.get_robot_config(robot_name) + current = self.get_current_joint_state(robot_name) + if plan is None or config is None or current is None: + return None + path: JointPath = [] + current_by_name = dict(zip(current.name, current.position, strict=False)) + for waypoint in plan.path: + selected = dict(zip(waypoint.name, waypoint.position, strict=False)) + positions: list[float] = [] + for local_name in config.joint_names: + global_name = f"{robot_name}/{local_name}" + if global_name in selected: + positions.append(float(selected[global_name])) + elif local_name in current_by_name: + positions.append(float(current_by_name[local_name])) + else: + return None + path.append(JointState(name=list(config.joint_names), position=positions)) + return path + + def get_planned_trajectory_duration(self, robot_name: RobotName) -> float | None: + path = self.get_planned_path(robot_name) + return None if path is None else float(max(len(path) - 1, 0)) + + def get_module_state(self) -> str: + return str(self._module.get_state()) + + def get_error(self) -> str: + return self._module.get_error() + + def reset(self) -> bool: + return self._module.reset().is_success() + + def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: + return self._module.plan_to_pose(pose, robot_name) + + def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: + return self._module.plan_to_joints(joints, robot_name) + + def preview_path(self, robot_name: RobotName | None = None) -> object: + preview_path = getattr(self._module, "preview_path", None) + if callable(preview_path): + return preview_path(robot_name=robot_name) + return self._module.preview_plan(robot_name=robot_name) + + def execute(self, robot_name: RobotName | None = None) -> bool: + execute_plan = getattr(self._module, "execute_plan", None) + if callable(execute_plan): + return bool(execute_plan()) + execute = getattr(self._module, "execute", None) + return bool(execute(robot_name)) if callable(execute) else False + + def cancel(self) -> bool: + return self._module.cancel() + + def clear_planned_path(self) -> bool: + return self._module.clear_planned_path() + + @staticmethod + def joints_from_values(joint_names: Sequence[str], values: Sequence[float]) -> JointState: + return JointState( + { + "name": list(joint_names), + "position": [float(value) for value in values], + } + ) diff --git a/dimos/manipulation/visualization/viser/animation.py b/dimos/manipulation/visualization/viser/animation.py index 03b4e3321e..f5a574d82c 100644 --- a/dimos/manipulation/visualization/viser/animation.py +++ b/dimos/manipulation/visualization/viser/animation.py @@ -15,31 +15,11 @@ from __future__ import annotations from collections.abc import Callable, Sequence -from dataclasses import dataclass import time -from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.msgs.sensor_msgs.JointState import JointState -@dataclass(frozen=True) -class PreviewTrack: - """One render track owned by one or more planning groups.""" - - robot_id: str - group_ids: tuple[PlanningGroupID, ...] - joint_names: tuple[str, ...] - path: tuple[JointState, ...] - - -@dataclass(frozen=True) -class GroupPreviewAnimation: - """Group-native preview transaction for a generated plan.""" - - group_ids: tuple[PlanningGroupID, ...] - tracks: tuple[PreviewTrack, ...] - - def interpolate_joint_path( path: Sequence[JointState], duration: float, fps: float ) -> list[list[float]]: @@ -77,10 +57,10 @@ def sampled_joint_path_frames( ) -> list[list[float]]: """Return animation frames while preserving already sampled trajectories. - ViserManipulationVisualizer.animate_plan() projects GeneratedPlan waypoints into robot-local - preview paths. If a path arrives already sampled near the target display rate, Viser should - play those samples directly instead of re-interpolating by waypoint index. Sparse direct scene - callers still get local interpolation as a fallback. + ManipulationModule.preview_path() owns trajectory-aware interpolation because it has access + to JointTrajectory waypoint timing. If a path arrives already sampled near the target display + rate, Viser should play those samples directly instead of re-interpolating by waypoint index. + Sparse direct VisualizationSpec callers still get local interpolation as a fallback. """ waypoints = [list(waypoint.position) for waypoint in path if waypoint.position] if not waypoints: @@ -112,8 +92,7 @@ def animate(self, path: Sequence[JointState], duration: float, fps: float) -> bo if not frames: return False step_delay = duration / max(len(frames) - 1, 1) if duration > 0.0 else 0.0 - for index, joints in enumerate(frames): + for joints in frames: self._set_joints(joints) - if index < len(frames) - 1: - self._sleep(step_delay) + self._sleep(step_delay) return True diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 175d900238..0434228583 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -14,29 +14,11 @@ from __future__ import annotations -from collections.abc import Callable, Mapping -from typing import TYPE_CHECKING, TypeAlias, cast - -from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName -from dimos.manipulation.visualization.types import ( - RobotInfo, - TargetEvaluation, - TargetSetEvaluation, -) +from typing import TypeAlias + +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.panel_backend import ( - copy_joint_state, - evaluate_joint_target_set, - evaluate_pose_target_set, - feasibility_status, - get_current_joint_state, - get_ee_pose, - is_state_stale, - joint_values_by_name, - pose_from_transform_values, - update_target_visual_state, -) from dimos.manipulation.visualization.viser.runtime import VISER_INSTALL_HINT from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( @@ -56,10 +38,6 @@ from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger -if TYPE_CHECKING: - from dimos.manipulation.manipulation_module import ManipulationModule - from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor - logger = setup_logger() try: @@ -90,23 +68,6 @@ # Fallback joint-slider range (radians) when a robot config omits joint limits. DEFAULT_JOINT_LIMITS = (-3.14, 3.14) -PRIMARY_ACTION_COLOR = (0, 102, 179) -ACTIVE_GROUP_COLOR = PRIMARY_ACTION_COLOR -INACTIVE_GROUP_COLOR = (52, 52, 52) - - -def group_display_name(group: PlanningGroup) -> str: - robot_name = group.robot_name - group_name = group.group_name - return robot_name if group_name == "manipulator" else f"{robot_name} {group_name}" - - -def group_selector_color( - selected: bool, - active_color: tuple[int, int, int], - inactive_color: tuple[int, int, int], -) -> tuple[int, int, int]: - return active_color if selected else inactive_color class ViserPanelGui: @@ -115,21 +76,18 @@ class ViserPanelGui: def __init__( self, server: ViserServer, - world_monitor: WorldMonitor, - manipulation_module: ManipulationModule, + adapter: InProcessViserAdapter, config: ViserVisualizationConfig, scene: ViserManipulationScene | None = None, ) -> None: self.server = server - self.world_monitor = world_monitor - self.manipulation_module = manipulation_module + self.adapter = adapter self.config = config self.scene = scene self.state = PanelState(runtime=PanelRuntime.STARTING) self._closed = False self._operation_sequence_id = 0 self._suppress_target_callbacks = False - self._default_group_initialized = False self._handles: dict[str, PanelHandle] = {} self._joint_sliders: dict[str, GuiSliderHandle[float]] = {} self._worker = TargetEvaluationWorker( @@ -169,93 +127,21 @@ def close(self) -> None: def refresh(self) -> None: if self._closed: return - robots = self._list_robots() - groups = self._list_planning_groups() + robots = self.adapter.list_robots() self.state.backend_status = ( BackendConnectionStatus.READY if robots else BackendConnectionStatus.WAITING_FOR_ROBOT ) - if not self.state.selected_group_ids and groups and not self._default_group_initialized: - first_pose_group = next((group for group in groups if group.has_pose_target), groups[0]) - self.state.selected_group_ids = (first_pose_group.id,) + if self.state.selected_robot is None and robots: + self.state.selected_robot = robots[0] self.state.target_status = TargetStatus.EMPTY - self._default_group_initialized = True - self._sync_group_selection_state() - self._initialize_selected_group_targets() self._build_joint_sliders() - self._sync_group_selector(groups) + self._sync_robot_dropdown(robots) self._refresh_selected_robot_state() self._ensure_scene_controls() - self._sync_target_ghost_visibility() self._sync_preset_dropdown() self._update_status_text() - self._update_target_summary() self._update_control_state() - def _list_robots(self) -> list[RobotName]: - return list(self.manipulation_module.list_robots()) - - def _list_planning_groups(self) -> list[PlanningGroup]: - return self.manipulation_module.list_planning_groups() - - def _get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: - return self.manipulation_module.get_robot_info(robot_name) - - def _get_init_joints(self, robot_name: RobotName) -> JointState | None: - return copy_joint_state(self.manipulation_module.get_init_joints(robot_name)) - - def _get_current_joint_state(self, robot_name: RobotName) -> JointState | None: - return get_current_joint_state( - self.world_monitor, - self.manipulation_module, - robot_name, - ) - - def _is_state_stale(self, robot_name: RobotName, max_age: float = 1.0) -> bool: - return is_state_stale( - self.world_monitor, - self.manipulation_module, - robot_name, - max_age, - ) - - def _get_ee_pose( - self, robot_name: RobotName, joint_state: JointState | None = None - ) -> Pose | None: - return get_ee_pose( - self.world_monitor, - self.manipulation_module, - self._list_planning_groups(), - robot_name, - joint_state, - ) - - def _get_module_state(self) -> str: - return str(self.manipulation_module.get_state()) - - def _reset(self) -> bool: - result = self.manipulation_module.reset() - return result if isinstance(result, bool) else result.is_success() - - def _evaluate_joint_target_set( - self, joint_targets: dict[PlanningGroupID, JointState] - ) -> TargetSetEvaluation: - return evaluate_joint_target_set(self.manipulation_module, joint_targets) - - def _evaluate_pose_target_set( - self, - pose_targets: dict[PlanningGroupID, Pose], - auxiliary_groups: tuple[PlanningGroupID, ...] = (), - seed: JointState | None = None, - check_collision: bool = True, - ) -> TargetSetEvaluation: - return evaluate_pose_target_set( - self.manipulation_module, - pose_targets, - auxiliary_groups=auxiliary_groups, - seed=seed, - check_collision=check_collision, - ) - def _build(self) -> None: gui = self.server.gui folder = gui.add_folder("Manipulation Panel", expand_by_default=True) @@ -264,28 +150,26 @@ def _build(self) -> None: self._build_panel_controls(gui) def _build_panel_controls(self, gui: GuiApi) -> None: - self._handles["status"] = gui.add_markdown("### Status\n**State:** Ready") + self._handles["status"] = gui.add_markdown("Starting manipulation panel...") + robots = self.adapter.list_robots() self._build_scene_controls(gui) - self._handles["planning_groups_heading"] = gui.add_markdown( - "### Planning Groups\nActive MoveIt group for pose goal, planning, and joint edits." + robot_dropdown = gui.add_dropdown( + "Robot", + options=robots or [""], + initial_value=robots[0] if robots else "", ) - self._sync_group_selector(self._list_planning_groups()) - self._handles["target_heading"] = gui.add_markdown("### Target") + robot_dropdown.on_update(lambda event: self._select_robot(event.target.value)) + self._handles["robot"] = robot_dropdown preset_dropdown = gui.add_dropdown( - "Preset", + "Target Preset", options=["Select preset...", "Current"], initial_value="Select preset...", ) preset_dropdown.on_update(lambda event: self._apply_preset(event.target.value)) self._handles["preset"] = preset_dropdown - self._handles["target_summary"] = gui.add_markdown( - f"Feasibility: `{self.state.feasibility.status.value}`" - ) - self._handles["actions_heading"] = gui.add_markdown("### Actions") - plan_button = gui.add_button("Plan", disabled=True, color=PRIMARY_ACTION_COLOR) + plan_button = gui.add_button("Plan", disabled=True) plan_button.on_click(lambda _: self._submit_plan()) self._handles["plan"] = plan_button - self._handles["plan_controls_heading"] = gui.add_markdown("**Plan controls**") preview_button = gui.add_button("Preview", disabled=True) preview_button.on_click(lambda _: self._submit_preview()) self._handles["preview"] = preview_button @@ -298,8 +182,6 @@ def _build_panel_controls(self, gui: GuiApi) -> None: clear_button = gui.add_button("Clear plan") clear_button.on_click(lambda _: self._submit_clear()) self._handles["clear"] = clear_button - joint_controls = gui.add_folder("Joint Control", expand_by_default=False) - self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() def _build_scene_controls(self, gui: GuiApi) -> None: @@ -324,152 +206,68 @@ def _refresh_selected_robot_state(self) -> None: self.state.robot_info = None self.state.current_joints = None self.state.current_ee_pose = None - self.state.manipulation_state = self._get_module_state() + self.state.manipulation_state = self.adapter.get_module_state() return - self.state.robot_info = self._get_robot_info(robot_name) - current = self._get_current_joint_state(robot_name) + self.state.robot_info = self.adapter.get_robot_info(robot_name) + current = self.adapter.get_current_joint_state(robot_name) self.state.current_joints = list(current.position) if current is not None else None - self.state.current_ee_pose = self._get_ee_pose(robot_name) - self.state.manipulation_state = self._get_module_state() - adapter_error = self.manipulation_module.get_error() + self.state.current_ee_pose = self.adapter.get_ee_pose(robot_name) + self.state.manipulation_state = self.adapter.get_module_state() + adapter_error = self.adapter.get_error() if adapter_error: self.state.error = adapter_error def _ensure_scene_controls(self) -> None: - if self.scene is None: + if self.scene is None or self.state.selected_robot is None: return - groups = self._group_info_by_id() - active_pose_groups = set(self._selected_pose_group_ids()) - for key in [key for key in self._handles if key.startswith("ee_control:")]: - group_id = key.split(":", 1)[1] - if group_id in active_pose_groups: - continue - handle = self._handles.pop(key) - remove_target_controls = getattr(self.scene, "remove_target_controls", None) - if callable(remove_target_controls): - remove_target_controls(group_id) - else: - remove = getattr(handle, "remove", None) - if callable(remove): - remove() - for group_id in active_pose_groups: - group = groups.get(group_id) - if group is None or not bool(group.has_pose_target): - continue - handle_key = f"ee_control:{group_id}" - if handle_key in self._handles: - continue - ee_control = self.scene.ensure_target_controls( - group_id, - cast( - "Callable[[TransformControlsHandle], None]", - lambda target, gid=group_id: self._on_transform_update(gid, target), - ), - ) - if ee_control is not None: - self._handles[handle_key] = ee_control - pose = self.state.pose_targets.get(group_id) - if pose is not None: - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(group_id, pose) - finally: - self._suppress_target_callbacks = False + robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) + if robot_id is None: + return + ee_control = self.scene.ensure_target_controls(str(robot_id), self._on_transform_update) + if ee_control is not None: + self._handles["ee_control"] = ee_control + if ( + self.state.target_status == TargetStatus.EMPTY + and self.state.current_ee_pose is not None + ): + self.state.cartesian_target = self.state.current_ee_pose + self._suppress_target_callbacks = True + try: + self.scene.set_target_pose(str(robot_id), self.state.current_ee_pose) + finally: + self._suppress_target_callbacks = False def _build_joint_sliders(self) -> None: - gui = self.server.gui - self._clear_joint_sliders() - if not self.state.selected_group_ids: + if self.state.selected_robot is None: return - joint_folder = self._handles.get("joint_control_folder") - if joint_folder is not None: - folder = cast("GuiFolderHandle", joint_folder) - with folder: - self._build_joint_slider_handles(gui) + gui = self.server.gui + config = self.adapter.get_robot_config(self.state.selected_robot) + if config is None: return - self._build_joint_slider_handles(gui) - - def _build_joint_slider_handles(self, gui: GuiApi) -> None: - groups = self._group_info_by_id() - target_by_name: dict[str, float] = {} - if self.state.target_joints is not None: - target_by_name.update( - zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) + current = self.adapter.get_current_joint_state(self.state.selected_robot) + values = list(current.position) if current is not None else [0.0] * len(config.joint_names) + self._clear_joint_sliders() + joint_limits_lower = config.joint_limits_lower + joint_limits_upper = config.joint_limits_upper + for index, joint_name in enumerate(config.joint_names): + lower, upper = DEFAULT_JOINT_LIMITS + if joint_limits_lower is not None and index < len(joint_limits_lower): + lower = joint_limits_lower[index] + if joint_limits_upper is not None and index < len(joint_limits_upper): + upper = joint_limits_upper[index] + handle = gui.add_slider( + joint_name, + min=float(lower), + max=float(upper), + step=0.001, + initial_value=float(values[index] if index < len(values) else 0.0), ) - for group_id in self.state.selected_group_ids: - group = groups.get(group_id) - if group is None: - continue - config = self.manipulation_module.get_robot_config(str(group.robot_name)) - current = self._get_current_joint_state(str(group.robot_name)) - current_by_name = joint_values_by_name(str(group.robot_name), current) - joint_limits_lower = config.joint_limits_lower if config is not None else None - joint_limits_upper = config.joint_limits_upper if config is not None else None - for index, (global_name, local_name) in enumerate( - zip(group.joint_names, group.local_joint_names, strict=False) - ): - joint_name = str(global_name) - local = str(local_name) - value = float( - target_by_name.get( - joint_name, - target_by_name.get( - local, current_by_name.get(joint_name, current_by_name.get(local, 0.0)) - ), - ) - ) - lower, upper = DEFAULT_JOINT_LIMITS - if joint_limits_lower is not None and index < len(joint_limits_lower): - lower = joint_limits_lower[index] - if joint_limits_upper is not None and index < len(joint_limits_upper): - upper = joint_limits_upper[index] - handle = gui.add_slider( - f"{group_id}/{local}", - min=float(lower), - max=float(upper), - step=0.001, - initial_value=value, - ) - def on_update(_event: object, name: str = joint_name) -> None: - self._on_joint_slider_update(name) - - handle.on_update(on_update) - self._joint_sliders[joint_name] = handle - - def _target_set_from_sliders(self) -> dict[PlanningGroupID, JointState] | None: - groups = self._group_info_by_id() - targets: dict[PlanningGroupID, JointState] = {} - for group_id in self.state.selected_group_ids: - group = groups.get(group_id) - if group is None: - self._set_error(f"Unknown planning group: {group_id}") - return None - names = [str(name) for name in group.joint_names] - positions: list[float] = [] - for name in names: - handle = self._joint_sliders.get(name) - if handle is None: - self._set_error(f"Missing target slider for {name}") - return None - positions.append(float(handle.value)) - targets[group_id] = JointState({"name": names, "position": positions}) - return targets - - def _split_target_joints_by_group(self, target_joints: JointState) -> None: - groups = self._group_info_by_id() - positions_by_name = dict(zip(target_joints.name, target_joints.position, strict=False)) - self.state.group_joint_targets.clear() - for group_id in self.state.selected_group_ids: - group = groups.get(group_id) - if group is None: - continue - names = [str(name) for name in group.joint_names] - if not all(name in positions_by_name for name in names): - continue - self.state.group_joint_targets[group_id] = JointState( - {"name": names, "position": [float(positions_by_name[name]) for name in names]} - ) + def on_update(_event: object, name: str = joint_name) -> None: + self._on_joint_slider_update(name) + + handle.on_update(on_update) + self._joint_sliders[joint_name] = handle def _clear_joint_sliders(self) -> None: for handle in self._joint_sliders.values(): @@ -486,219 +284,51 @@ def _remove_panel_handles(self) -> None: remove() self._handles.pop(key, None) - def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: - seen_keys: set[str] = set() - selected = set(self.state.selected_group_ids) - for group in sorted( - groups, key=lambda item: (not bool(item.has_pose_target), str(item.id)) - ): - group_id = str(group.id) - key = f"group:{group_id}" - seen_keys.add(key) - handle = self._handles.get(key) - is_selected = group_id in selected - label = group_display_name(group) - if handle is None: - handle = self.server.gui.add_button( - label, - color=group_selector_color( - is_selected, ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR - ), - hint="Click to toggle this planning group in the target set.", - ) - handle.on_click( - cast( - "Callable[[object], None]", - lambda _event, gid=group_id: self._toggle_group_selected(gid), - ) - ) - self._handles[key] = handle - else: - self._set_optional_handle_attr(handle, "label", label) - self._set_optional_handle_attr( - handle, - "color", - group_selector_color(is_selected, ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR), - ) - - for key in [key for key in self._handles if key.startswith("group:")]: - if key not in seen_keys: - handle = self._handles.pop(key) - remove = getattr(handle, "remove", None) - if callable(remove): - remove() - - def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None: - current = list(self.state.selected_group_ids) - if selected and group_id not in current: - current.append(group_id) - elif not selected and group_id in current: - current.remove(group_id) - self.state.selected_group_ids = tuple(current) - self._sync_group_selection_state() - self._prune_inactive_group_state() - self._initialize_selected_group_targets() - self.state.mark_plan_stale() - self._build_joint_sliders() - self.refresh() - - def _toggle_group_selected(self, group_id: PlanningGroupID) -> None: - self._set_group_selected(group_id, group_id not in self.state.selected_group_ids) - - def _select_all_manipulators(self) -> None: - groups = self._list_planning_groups() - manipulator_groups = [ - str(group.id) for group in groups if str(group.group_name) == "manipulator" - ] - self.state.selected_group_ids = tuple( - manipulator_groups or [str(group.id) for group in groups] - ) - self._sync_group_selection_state() - self._initialize_selected_group_targets() - self._build_joint_sliders() - self.refresh() - - def _clear_group_selection(self) -> None: + def _select_robot(self, robot_name: str) -> None: if self._closed: return - self.state.selected_group_ids = () - self._sync_group_selection_state() - self._prune_inactive_group_state() + if (robot_name or None) == self.state.selected_robot: + self.refresh() + return + self.state.selected_robot = robot_name or None self.state.target_status = TargetStatus.EMPTY self.state.feasibility.status = FeasibilityStatus.UNKNOWN self.state.plan_state = PanelPlanState() self._build_joint_sliders() + self._sync_preset_dropdown() self.refresh() - def _group_info_by_id(self) -> dict[PlanningGroupID, PlanningGroup]: - return {str(group.id): group for group in self._list_planning_groups()} - - def _sync_selected_robot_from_groups(self) -> None: - groups = self._group_info_by_id() - first_group = ( - groups.get(self.state.selected_group_ids[0]) if self.state.selected_group_ids else None - ) - self.state.selected_robot = None if first_group is None else str(first_group.robot_name) - - def _sync_group_selection_state(self) -> None: - self._sync_selected_robot_from_groups() - self.state.auxiliary_group_ids = self._selected_auxiliary_group_ids() - - def _selected_pose_group_ids(self) -> tuple[PlanningGroupID, ...]: - groups = self._group_info_by_id() - return tuple( - group_id - for group_id in self.state.selected_group_ids - if (group := groups.get(group_id)) is not None and bool(group.has_pose_target) - ) - - def _selected_auxiliary_group_ids(self) -> tuple[PlanningGroupID, ...]: - groups = self._group_info_by_id() - return tuple( - group_id - for group_id in self.state.selected_group_ids - if (group := groups.get(group_id)) is not None and not bool(group.has_pose_target) - ) - - def _active_pose_targets(self) -> dict[PlanningGroupID, Pose]: - return { - group_id: self.state.pose_targets[group_id] - for group_id in self._selected_pose_group_ids() - if group_id in self.state.pose_targets - } - - def _prune_inactive_group_state(self) -> None: - selected = set(self.state.selected_group_ids) - for mapping in ( - self.state.pose_targets, - self.state.group_joint_targets, - self.state.group_poses, - self.state.group_diagnostics, - ): - for group_id in [group_id for group_id in mapping if group_id not in selected]: - mapping.pop(group_id, None) - self._refresh_target_joints_from_groups() - - def _initialize_selected_group_targets(self) -> None: - groups = self._group_info_by_id() - for group_id in self.state.selected_group_ids: - if group_id in self.state.group_joint_targets: - continue - group = groups.get(group_id) - if group is None: - continue - current = self._get_current_joint_state(str(group.robot_name)) - if current is None: - continue - current_by_name = joint_values_by_name(str(group.robot_name), current) - names = [str(name) for name in group.joint_names] - local_names = [str(name) for name in group.local_joint_names] - positions = [ - float(current_by_name.get(global_name, current_by_name.get(local, 0.0))) - for global_name, local in zip(names, local_names, strict=False) - ] - self.state.group_joint_targets[group_id] = JointState( - {"name": names, "position": positions} - ) - if bool(group.has_pose_target) and group_id not in self.state.pose_targets: - pose = self._get_ee_pose(str(group.robot_name)) - if pose is not None: - self.state.pose_targets[group_id] = pose - self.state.group_poses[group_id] = pose - if self.state.cartesian_target is None: - self.state.cartesian_target = pose - self._refresh_target_joints_from_groups() - - def _refresh_target_joints_from_groups(self) -> None: - names: list[str] = [] - positions: list[float] = [] - for group_id in self.state.selected_group_ids: - target = self.state.group_joint_targets.get(group_id) - if target is None: - continue - names.extend(target.name) - positions.extend(target.position) - self.state.target_joints = ( - JointState({"name": names, "position": positions}) if names else None - ) - - def _current_snapshot_by_group(self) -> dict[PlanningGroupID, list[float]]: - groups = self._group_info_by_id() - snapshot: dict[PlanningGroupID, list[float]] = {} - for group_id in self.state.selected_group_ids: - group = groups.get(group_id) - if group is None: - continue - current = self._get_current_joint_state(str(group.robot_name)) - if current is None: - continue - current_by_name = joint_values_by_name(str(group.robot_name), current) - snapshot[group_id] = [ - float( - current_by_name.get(str(global_name), current_by_name.get(str(local_name), 0.0)) - ) - for global_name, local_name in zip( - group.joint_names, group.local_joint_names, strict=False - ) - ] - return snapshot + def _sync_robot_dropdown(self, robots: list[str]) -> None: + handle = self._handles.get("robot") + if handle is None: + return + options = robots or [""] + for attr in ("options", "values"): + if hasattr(handle, attr): + try: + self._set_optional_handle_attr(handle, attr, options) + except Exception: + logger.warning("Could not set robot dropdown %s", attr, exc_info=True) + if hasattr(handle, "value") and self.state.selected_robot in robots: + try: + self._set_optional_handle_attr(handle, "value", self.state.selected_robot) + except Exception: + logger.warning("Could not set robot dropdown value", exc_info=True) def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") - if handle is None: + if handle is None or self.state.selected_robot is None: return - selected_robot_names = self._selected_robot_names() + info: RobotInfo | None = self.adapter.get_robot_info(self.state.selected_robot) + config = self.adapter.get_robot_config(self.state.selected_robot) options = ["Select preset..."] - if any( - self._get_init_joints(robot_name) is not None for robot_name in selected_robot_names - ): + if (info is not None and info["init_joints"] is not None) or self.adapter.get_init_joints( + self.state.selected_robot + ) is not None: options.append("Init") options.append("Current") - if any( - (config := self.manipulation_module.get_robot_config(robot_name)) is not None - and config.home_joints is not None - for robot_name in selected_robot_names - ): + home_joints = config.home_joints if config is not None else None + if (info is not None and info["home_joints"] is not None) or home_joints is not None: options.append("Home") for attr in ("options", "values"): if hasattr(handle, attr): @@ -710,64 +340,27 @@ def _sync_preset_dropdown(self) -> None: def _apply_preset(self, preset: str) -> None: if self._closed: return - if preset not in {"Current", "Init", "Home"}: + robot_name = self.state.selected_robot + if robot_name is None: return - groups = [ - group - for group in self._list_planning_groups() - if group.id in self.state.selected_group_ids - ] - for group in groups: - robot_name = str(group.robot_name) - values_by_name = self._preset_values_by_name(preset, robot_name) - global_names = [str(name) for name in group.joint_names] - local_names = [str(name) for name in group.local_joint_names] - values = [ - float(values_by_name.get(local_name, values_by_name.get(global_name, 0.0))) - for local_name, global_name in zip(local_names, global_names, strict=False) - ] - self._set_slider_values(global_names, values) - self.state.joint_target = [float(handle.value) for handle in self._joint_sliders.values()] + config = self.adapter.get_robot_config(robot_name) + if config is None: + return + if preset == "Current": + current = self.adapter.get_current_joint_state(robot_name) + values = list(current.position) if current is not None else [] + elif preset == "Init": + init = self.adapter.get_init_joints(robot_name) + values = list(init.position) if init is not None else [] + elif preset == "Home": + values = list(config.home_joints or []) + else: + return + self._set_slider_values(config.joint_names, values) + self.state.joint_target = [float(value) for value in values] self._submit_joint_target_evaluation() self.refresh() - def _selected_robot_names(self) -> tuple[str, ...]: - groups = self._group_info_by_id() - names: list[str] = [] - for group_id in self.state.selected_group_ids: - group = groups.get(group_id) - if group is None: - continue - robot_name = str(group.robot_name) - if robot_name not in names: - names.append(robot_name) - return tuple(names) - - def _preset_values_by_name(self, preset: str, robot_name: str) -> dict[str, float]: - if preset == "Current": - current = self._get_current_joint_state(robot_name) - if current is None: - return {} - return { - str(name): float(value) - for name, value in zip(current.name, current.position, strict=False) - } - if preset == "Init": - init = self._get_init_joints(robot_name) - if init is None: - return {} - return { - str(name): float(value) - for name, value in zip(init.name, init.position, strict=False) - } - config = self.manipulation_module.get_robot_config(robot_name) - if config is None: - return {} - return { - str(name): float(value) - for name, value in zip(config.joint_names, config.home_joints or [], strict=False) - } - def _set_slider_values(self, joint_names: list[str], values: list[float]) -> None: self._suppress_target_callbacks = True try: @@ -778,6 +371,18 @@ def _set_slider_values(self, joint_names: list[str], values: list[float]) -> Non finally: self._suppress_target_callbacks = False + def _target_from_sliders(self, robot_name: str) -> JointState | None: + config = self.adapter.get_robot_config(robot_name) + if config is None: + self._set_error("No robot config") + return None + values = [ + float(self._joint_sliders[name].value) + for name in config.joint_names + if name in self._joint_sliders + ] + return self.adapter.joints_from_values(config.joint_names, values) + def _on_joint_slider_update(self, _joint_name: str) -> None: if self._closed: return @@ -785,104 +390,73 @@ def _on_joint_slider_update(self, _joint_name: str) -> None: return self._submit_joint_target_evaluation() - def _on_transform_update( - self, group_id: PlanningGroupID, target: TransformControlsHandle - ) -> None: + def _on_transform_update(self, target: TransformControlsHandle) -> None: if self._closed: return - if self._suppress_target_callbacks or group_id not in self.state.selected_group_ids: + if self._suppress_target_callbacks or self.state.selected_robot is None: + return + pose = self._pose_from_transform_target(target) + if pose is None: return - pose = pose_from_transform_values(target.position.tolist(), target.wxyz.tolist()) self.state.cartesian_target = pose - self.state.pose_targets[group_id] = pose sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="cartesian", - group_ids=self.state.selected_group_ids, - auxiliary_group_ids=self._selected_auxiliary_group_ids(), - pose_targets=self._active_pose_targets(), - check_collision=True, + robot_name=self.state.selected_robot, + pose=pose, ) ) self.refresh() def _submit_joint_target_evaluation(self) -> None: - targets = self._target_set_from_sliders() - if targets is None: + robot_name = self.state.selected_robot + if robot_name is None: + return + target = self._target_from_sliders(robot_name) + if target is None: return - self.state.group_joint_targets = targets - self._refresh_target_joints_from_groups() - self._move_joint_target_visuals() + self.state.joint_target = list(target.position) + self._move_joint_target_visuals(robot_name, target) sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - group_ids=self.state.selected_group_ids, - joint_targets=dict(targets), + robot_name=robot_name, + joints=target, ) ) self.refresh() - def _move_joint_target_visuals(self) -> None: + def _move_joint_target_visuals(self, robot_name: str, target: JointState) -> None: """Optimistically move target visuals before collision/feasibility returns.""" - if self.scene is None: - return - groups = self._group_info_by_id() - for group_id, target in self.state.group_joint_targets.items(): - group = groups.get(group_id) - if group is None: - continue - robot_name = str(group.robot_name) - robot_id = self.manipulation_module.robot_id_for_name(robot_name) - config = self.manipulation_module.get_robot_config(robot_name) - if robot_id is None or config is None: - continue - local_positions = dict(zip(target.name, target.position, strict=False)) - joints = [ - float(local_positions.get(str(global_name), 0.0)) - for global_name in group.joint_names - ] - self.scene.set_target_joints(str(robot_id), list(group.local_joint_names), joints) - - def _sync_target_ghost_visibility(self) -> None: - if self.scene is None: - return - active_robot_ids: set[str] = set() - groups = self._group_info_by_id() - for group_id in self._selected_pose_group_ids(): - group = groups.get(group_id) - if group is None: - continue - robot_id = self.manipulation_module.robot_id_for_name(str(group.robot_name)) - if robot_id is not None: - active_robot_ids.add(str(robot_id)) - set_target_active = getattr(self.scene, "set_target_active", None) - if not callable(set_target_active): - return - for _robot_name, robot_id, _config in self.manipulation_module.robot_items(): - set_target_active(str(robot_id), str(robot_id) in active_robot_ids) + config = self.adapter.get_robot_config(robot_name) + robot_id = self.adapter.robot_id_for_name(robot_name) + if self.scene is not None and config is not None and robot_id is not None: + self.scene.set_target_joints(str(robot_id), config.joint_names, list(target.position)) + pose = self.adapter.get_ee_pose(robot_name, target) + if pose is not None: + self._suppress_target_callbacks = True + try: + self.scene.set_target_pose(str(robot_id), pose) + finally: + self._suppress_target_callbacks = False def _handle_target_evaluation_request( self, request: TargetEvaluationRequest - ) -> TargetEvaluation | TargetSetEvaluation: + ) -> TargetEvaluation: if request.source == "cartesian": - if not request.pose_targets: + if request.pose is None: return {"success": False, "status": "INVALID", "message": "No pose target"} - return self._evaluate_pose_target_set( - request.pose_targets, - auxiliary_groups=request.auxiliary_group_ids, - seed=self.state.last_valid_target_joints, - check_collision=request.check_collision, - ) - if not request.joint_targets: + return self.adapter.evaluate_pose_target(request.pose, request.robot_name) + if request.joints is None: return {"success": False, "status": "INVALID", "message": "No joint target"} - return self._evaluate_joint_target_set(request.joint_targets) + return self.adapter.evaluate_joint_target(request.joints, request.robot_name) def _apply_target_evaluation_result( - self, request: TargetEvaluationRequest, result: TargetEvaluation | TargetSetEvaluation + self, request: TargetEvaluationRequest, result: TargetEvaluation ) -> None: if self._closed: return @@ -890,100 +464,68 @@ def _apply_target_evaluation_result( return collision_free = bool(result.get("collision_free", False)) success = bool(result.get("success", False)) - self.state.feasibility.status = feasibility_status( - str(result.get("status", "")), success, collision_free - ) + self.state.feasibility.status = self._feasibility_status(result, success, collision_free) self.state.feasibility.message = str(result.get("message", "")) self.state.target_status = ( TargetStatus.FEASIBLE if success and collision_free else TargetStatus.INFEASIBLE ) self.state.error = "" if success and collision_free else self.state.feasibility.message - target_joints = result.get("target_joints") or result.get("joint_state") - if isinstance(target_joints, JointState): - self.state.target_joints = JointState(target_joints) - self._split_target_joints_by_group(target_joints) - if success and collision_free: - self.state.last_valid_target_joints = JointState(target_joints) - group_poses = result.get("group_poses", {}) - if isinstance(group_poses, dict): - self.state.group_poses = { - str(group_id): pose - for group_id, pose in group_poses.items() - if isinstance(pose, Pose) - } - if request.source == "joints" and isinstance(target_joints, JointState): - self._sync_pose_targets_from_group_poses() - group_diagnostics = result.get("group_diagnostics", {}) - if isinstance(group_diagnostics, dict): - self.state.group_diagnostics = { - str(group_id): str(message) for group_id, message in group_diagnostics.items() - } - if request.source == "cartesian" and isinstance(target_joints, JointState): + if request.source == "joints": + joint_state = result.get("joint_state") + if isinstance(joint_state, JointState): + self.state.joint_target = list(joint_state.position) + if request.source == "cartesian": + joint_state = result.get("joint_state") + if isinstance(joint_state, JointState): + self.state.joint_target = list(joint_state.position) + pose = result.get("ee_pose") + if isinstance(pose, Pose): + self.state.cartesian_target = pose self._sync_controls_from_targets() self._update_target_visual_state() self.refresh() def _sync_controls_from_targets(self) -> None: - if self.state.target_joints is not None: - positions_by_name = dict( - zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) - ) - self._set_slider_values(list(positions_by_name), list(positions_by_name.values())) - self._move_joint_target_visuals() + robot_name = self.state.selected_robot + if robot_name is None: + return + config = self.adapter.get_robot_config(robot_name) + if config is not None and self.state.joint_target is not None: + self._set_slider_values(list(config.joint_names), list(self.state.joint_target)) + robot_id = self.adapter.robot_id_for_name(robot_name) + if self.scene is not None and robot_id is not None: + self.scene.set_target_joints( + str(robot_id), config.joint_names, self.state.joint_target + ) # Do not write the Cartesian target back into the active transform # control here. The gizmo is the source of truth for Cartesian edits; # programmatic pose writes from delayed IK results can fight fast user # dragging and make the gizmo jump back. - def _sync_pose_targets_from_group_poses(self) -> None: - groups = self._group_info_by_id() - updated_group_ids: list[PlanningGroupID] = [] - for group_id, pose in self.state.group_poses.items(): - group = groups.get(group_id) - if group is None or not bool(group.has_pose_target): - continue - if group_id not in self._selected_pose_group_ids(): - continue - self.state.pose_targets[group_id] = pose - updated_group_ids.append(group_id) - first_group_id = next(iter(self._selected_pose_group_ids()), None) - if first_group_id is not None: - self.state.cartesian_target = self.state.pose_targets.get(first_group_id) - self._sync_scene_target_pose_controls(updated_group_ids) - - def _sync_scene_target_pose_controls(self, group_ids: list[PlanningGroupID]) -> None: - if self.scene is None: - return - self._suppress_target_callbacks = True - try: - for group_id in group_ids: - pose = self.state.pose_targets.get(group_id) - if pose is not None: - self.scene.set_target_pose(group_id, pose) - finally: - self._suppress_target_callbacks = False - def _update_status_text(self) -> None: current = self.state.current_joints - status_label = self.state.error or self.state.module_state status = [ - "### Status", - f"**State:** {status_label}", - f"Target: `{self.state.target_status.value}` · Plan: `{self.state.plan_state.status.value}`", + "### Manipulation Panel", + f"Robot: `{self.state.selected_robot or 'none'}`", + f"Module: `{self.state.module_state}`", + f"Backend: `{self.state.backend_status.value}`", + f"Target: `{self.state.target_status.value}`", + f"Feasibility: `{self.state.feasibility.status.value}`", + f"Plan: `{self.state.plan_state.status.value}`", + f"Action: `{self.state.action_status.value}`", ] if self.state.selected_robot is not None: - status.append(f"State stale: `{self._is_state_stale(self.state.selected_robot)}`") + status.append( + f"State stale: `{self.adapter.is_state_stale(self.state.selected_robot)}`" + ) if current is not None: status.append(f"Current joints: `{[round(v, 3) for v in current]}`") if self.state.last_result: status.append(f"Last result: `{self.state.last_result}`") + if self.state.error: + status.append(f"Error: `{self.state.error}`") self._set_handle_value("status", "\n\n".join(status)) - def _update_target_summary(self) -> None: - self._set_handle_value( - "target_summary", f"Feasibility: `{self.state.feasibility.status.value}`" - ) - def _update_control_state(self) -> None: self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) @@ -994,29 +536,24 @@ def _update_control_state(self) -> None: and self.state.can_execute(self.config.current_match_tolerance) ), ) - can_cancel = self.state.can_cancel() - self._set_disabled("cancel", not can_cancel) - self._set_visible("cancel", can_cancel) + self._set_disabled("cancel", not self.state.can_cancel()) self._update_target_visual_state() def _update_target_visual_state(self) -> None: - if self.scene is None: + if self.scene is None or self.state.selected_robot is None: + return + robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) + if robot_id is None: return - update_target_visual_state( - self.scene, - self._group_info_by_id(), - self.state.selected_group_ids, - self.manipulation_module.robot_id_for_name, - self.state.feasibility.status == FeasibilityStatus.FEASIBLE, + self.scene.set_target_visual_state( + str(robot_id), self.state.feasibility.status == FeasibilityStatus.FEASIBLE ) def _submit_plan(self) -> None: if self._closed: return - if not self.state.selected_group_ids: - self._set_recoverable_error( - "Cannot plan until target is feasible and manipulation is idle" - ) + robot_name = self.state.selected_robot + if robot_name is None: return if not self.state.can_plan(): self._set_recoverable_error( @@ -1030,32 +567,28 @@ def operation() -> None: return self.state.action_status = ActionStatus.RUNNING self.state.plan_state.status = PlanStatus.PLANNING - if self.state.manipulation_state == "FAULT" and not self._reset(): + if self.state.manipulation_state == "FAULT" and not self.adapter.reset(): self.state.plan_state.status = PlanStatus.FAILED self._finish_operation("reset=False", clear_error=False, operation_id=operation_id) return - targets = self._target_set_from_sliders() - if targets is None: + target = self._target_from_sliders(robot_name) + if target is None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation( "plan_to_joints=False", clear_error=False, operation_id=operation_id ) return - ok = self.manipulation_module.plan_to_joint_targets( - cast("Mapping[PlanningGroupID | PlanningGroup, JointState]", targets) - ) + ok = self.adapter.plan_to_joints(target, robot_name) if not self._operation_is_current(operation_id): return if ok: + path = self.adapter.get_planned_path(robot_name) self.state.plan_state.status = PlanStatus.FRESH - self.state.plan_state.group_ids = self.state.selected_group_ids - self.state.plan_state.robot = self.state.selected_robot - self.state.plan_state.target_joints = list( - self.state.target_joints.position if self.state.target_joints else [] - ) + self.state.plan_state.robot = robot_name + self.state.plan_state.target_joints = list(target.position) self.state.plan_state.target_pose = self.state.cartesian_target - self.state.plan_state.start_joints_snapshot = self._current_snapshot_by_group() - self.state.plan_state.planned_path = None + self.state.plan_state.start_joints_snapshot = list(self.state.current_joints or []) + self.state.plan_state.planned_path = path else: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation(f"plan_to_joints={ok}", operation_id=operation_id) @@ -1067,6 +600,9 @@ def operation() -> None: def _submit_preview(self) -> None: if self._closed: return + robot_name = self.state.selected_robot + if robot_name is None: + return if not self.state.can_preview(): self._set_recoverable_error("No fresh plan to preview") return @@ -1076,7 +612,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.PREVIEWING - ok = self.manipulation_module.preview_plan() + ok = self.adapter.preview_path(robot_name) self._finish_operation(f"preview={ok}", operation_id=operation_id) self._operation_worker.submit( @@ -1088,6 +624,9 @@ def operation() -> None: def _submit_execute(self) -> None: if self._closed: return + robot_name = self.state.selected_robot + if robot_name is None: + return if not self.config.allow_plan_execute: self._set_recoverable_error( "Panel execution disabled; set allow_plan_execute=True to enable" @@ -1105,7 +644,7 @@ def operation() -> None: return self.state.action_status = ActionStatus.EXECUTING self.state.plan_state.status = PlanStatus.EXECUTING - ok = self.manipulation_module.execute() + ok = self.adapter.execute(robot_name) if not self._operation_is_current(operation_id): return if not ok: @@ -1127,7 +666,7 @@ def _submit_cancel(self) -> None: self._mark_cancelled_plan_state(cancelled_action) self._restart_operation_worker() try: - ok = self.manipulation_module.cancel() + ok = self.adapter.cancel() except Exception as e: self._set_operation_error(str(e), operation_id) return @@ -1156,7 +695,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.CLEARING_PLAN - ok = self.manipulation_module.clear_planned_path() + ok = self.adapter.clear_planned_path() if not self._operation_is_current(operation_id): return self.state.plan_state = PanelPlanState() @@ -1206,22 +745,31 @@ def _set_error(self, message: str) -> None: def _set_handle_value(self, key: str, value: str) -> None: handle = self._handles.get(key) - if handle is None: - return - if hasattr(handle, "content") or hasattr(handle, "value"): - attr = "content" if hasattr(handle, "content") else "value" - self._set_optional_handle_attr(handle, attr, value) + if isinstance(handle, GuiMarkdownHandle): + self._set_optional_handle_attr(handle, "value", value) def _set_disabled(self, key: str, disabled: bool) -> None: handle = self._handles.get(key) - if handle is not None and hasattr(handle, "disabled"): + if isinstance(handle, GuiButtonHandle): self._set_optional_handle_attr(handle, "disabled", disabled) - def _set_visible(self, key: str, visible: bool) -> None: - handle = self._handles.get(key) - if handle is not None: - self._set_optional_handle_attr(handle, "visible", visible) - @staticmethod def _set_optional_handle_attr(handle: object, attr: str, value: object) -> None: setattr(handle, attr, value) + + def _pose_from_transform_target(self, target: TransformControlsHandle) -> Pose | None: + px, py, pz = (float(value) for value in target.position) + qw, qx, qy, qz = (float(value) for value in target.wxyz) + return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) + + def _feasibility_status( + self, result: TargetEvaluation, success: bool, collision_free: bool + ) -> FeasibilityStatus: + status = str(result.get("status", "")).upper() + if success and collision_free: + return FeasibilityStatus.FEASIBLE + if status in {"COLLISION", "COLLISION_AT_START", "COLLISION_AT_GOAL"}: + return FeasibilityStatus.COLLISION + if status in {"NO_SOLUTION", "SINGULARITY", "JOINT_LIMITS", "TIMEOUT"}: + return FeasibilityStatus.IK_FAILED + return FeasibilityStatus.INVALID diff --git a/dimos/manipulation/visualization/viser/panel_backend.py b/dimos/manipulation/visualization/viser/panel_backend.py deleted file mode 100644 index 6fbc51a408..0000000000 --- a/dimos/manipulation/visualization/viser/panel_backend.py +++ /dev/null @@ -1,268 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Callable, Mapping, Sequence -from typing import TYPE_CHECKING, cast - -from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName -from dimos.manipulation.visualization.types import TargetSetEvaluation -from dimos.manipulation.visualization.viser.state import FeasibilityStatus -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState - -if TYPE_CHECKING: - from dimos.manipulation.manipulation_module import ManipulationModule - from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor - - -def copy_joint_state(joint_state: JointState | None) -> JointState | None: - return None if joint_state is None else JointState(joint_state) - - -def get_current_joint_state( - world_monitor: WorldMonitor, - manipulation_module: ManipulationModule, - robot_name: RobotName, -) -> JointState | None: - current = manipulation_module.get_current_joint_state(robot_name) - if current is None: - robot_id = manipulation_module.robot_id_for_name(robot_name) - if robot_id is not None: - current = world_monitor.get_current_joint_state(robot_id) - return copy_joint_state(current) - - -def is_state_stale( - world_monitor: WorldMonitor, - manipulation_module: ManipulationModule, - robot_name: RobotName, - max_age: float = 1.0, -) -> bool: - robot_id = manipulation_module.robot_id_for_name(robot_name) - return True if robot_id is None else world_monitor.is_state_stale(robot_id, max_age) - - -def get_ee_pose( - world_monitor: WorldMonitor, - manipulation_module: ManipulationModule, - groups: Sequence[PlanningGroup], - robot_name: RobotName, - joint_state: JointState | None = None, -) -> Pose | None: - group_id = primary_pose_group_id(groups, robot_name) - if group_id is None: - return None - copied = copy_joint_state(joint_state) - fk = manipulation_module.forward_kinematics(group_id, copied) - if fk.pose is not None: - return cast("Pose", fk.pose) - robot_id = manipulation_module.robot_id_for_name(robot_name) - if robot_id is None: - return None - get_world_ee_pose = getattr(world_monitor, "get_ee_pose", None) - if callable(get_world_ee_pose): - return cast("Pose | None", get_world_ee_pose(robot_id, copy_joint_state(joint_state))) - return None - - -def evaluate_joint_target_set( - manipulation_module: ManipulationModule, - joint_targets: Mapping[PlanningGroupID, JointState], -) -> TargetSetEvaluation: - if not joint_targets: - return {"success": False, "status": "INVALID", "message": "No joint target"} - names: list[str] = [] - positions: list[float] = [] - for target in joint_targets.values(): - copied = copy_joint_state(target) - if copied is None: - continue - names.extend(str(name) for name in copied.name) - positions.extend(float(position) for position in copied.position) - return evaluate_global_target_set( - manipulation_module, - tuple(joint_targets), - JointState({"name": names, "position": positions}), - ) - - -def evaluate_pose_target_set( - manipulation_module: ManipulationModule, - pose_targets: Mapping[PlanningGroupID, Pose], - auxiliary_groups: Sequence[PlanningGroupID] = (), - seed: JointState | None = None, - check_collision: bool = True, -) -> TargetSetEvaluation: - if not pose_targets: - return {"success": False, "status": "INVALID", "message": "No pose target"} - stamped_targets = {group_id: stamped_pose(pose) for group_id, pose in pose_targets.items()} - group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_groups))) - ik = manipulation_module.inverse_kinematics( - pose_targets=stamped_targets, - auxiliary_group_ids=auxiliary_groups, - seed=copy_joint_state(seed), - ) - if not ik.is_success() or ik.joint_state is None: - return { - "success": False, - "status": ik.status.name, - "message": ik.message, - "collision_free": False, - "group_ids": group_ids, - "target_joints": None, - "position_error": ik.position_error, - "orientation_error": ik.orientation_error, - } - return evaluate_global_target_set( - manipulation_module, - group_ids, - ik.joint_state, - status=ik.status.name, - message=ik.message, - position_error=ik.position_error, - orientation_error=ik.orientation_error, - check_collision=check_collision, - ) - - -def evaluate_global_target_set( - manipulation_module: ManipulationModule, - group_ids: tuple[PlanningGroupID, ...], - target_joints: JointState, - *, - status: str = "FEASIBLE", - message: str = "Target is collision-free", - position_error: float = 0.0, - orientation_error: float = 0.0, - check_collision: bool = True, -) -> TargetSetEvaluation: - target = copy_joint_state(target_joints) - if target is None: - return {"success": False, "status": "INVALID", "message": "No target joints"} - collision = manipulation_module.check_collision(target) if check_collision else None - collision_free = True if collision is None else collision.collision_free is True - diagnostics = { - group_id: "Target collision check skipped" if collision is None else collision.message - for group_id in group_ids - } - result_message = message - if collision is None: - result_message = "Target collision check skipped" - elif not collision_free: - result_message = collision.message - group_poses: dict[PlanningGroupID, Pose | None] = {} - for group_id in group_ids: - fk = manipulation_module.forward_kinematics(group_id, target) - if fk.pose is not None: - group_poses[group_id] = cast("Pose", fk.pose) - elif fk.status != "INVALID": - group_poses[group_id] = None - return { - "success": collision_free, - "status": status - if collision_free - else collision.status - if collision is not None - else "COLLISION", - "message": result_message, - "collision_free": collision_free, - "group_ids": group_ids, - "target_joints": target, - "group_diagnostics": diagnostics, - "group_poses": group_poses, - "position_error": position_error, - "orientation_error": orientation_error, - } - - -def primary_pose_group_id( - groups: Sequence[PlanningGroup], robot_name: RobotName -) -> PlanningGroupID | None: - for group in groups: - if group.robot_name == robot_name and group.has_pose_target: - return group.id - return None - - -def stamped_pose(pose: Pose | PoseStamped) -> PoseStamped: - if isinstance(pose, PoseStamped): - return pose - return PoseStamped(frame_id="world", position=pose.position, orientation=pose.orientation) - - -def joint_values_by_name(robot_name: str, joint_state: JointState | None) -> dict[str, float]: - if joint_state is None: - return {} - values: dict[str, float] = {} - for name, position in zip(joint_state.name, joint_state.position, strict=False): - name_str = str(name) - position_float = float(position) - values[name_str] = position_float - if "/" in name_str: - values[name_str.rsplit("/", 1)[1]] = position_float - else: - values[f"{robot_name}/{name_str}"] = position_float - return values - - -def pose_from_transform_values(position: Sequence[float], wxyz: Sequence[float]) -> Pose: - px, py, pz = (float(value) for value in position) - qw, qx, qy, qz = (float(value) for value in wxyz) - return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) - - -def feasibility_status( - status: str, - success: bool, - collision_free: bool, -) -> FeasibilityStatus: - normalized = status.upper() - if success and collision_free: - return FeasibilityStatus.FEASIBLE - if normalized in {"COLLISION", "COLLISION_AT_START", "COLLISION_AT_GOAL"}: - return FeasibilityStatus.COLLISION - if normalized in {"NO_SOLUTION", "SINGULARITY", "JOINT_LIMITS", "TIMEOUT"}: - return FeasibilityStatus.IK_FAILED - return FeasibilityStatus.INVALID - - -def update_target_visual_state( - scene: object, - groups: Mapping[PlanningGroupID, PlanningGroup], - selected_group_ids: Sequence[PlanningGroupID], - robot_id_for_name: Callable[[RobotName], object | None], - feasible: bool, -) -> None: - set_visual_state = getattr(scene, "set_target_visual_state", None) - if not callable(set_visual_state): - return - updated: set[str] = set() - for group_id in selected_group_ids: - group_id_str = str(group_id) - if group_id_str not in updated: - set_visual_state(group_id_str, feasible) - updated.add(group_id_str) - group = groups.get(group_id_str) - if group is None: - continue - robot_id = robot_id_for_name(str(group.robot_name)) - robot_id_str = None if robot_id is None else str(robot_id) - if robot_id_str is None or robot_id_str in updated: - continue - set_visual_state(robot_id_str, feasible) - updated.add(robot_id_str) diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 049ffdf659..cd50ab098e 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -16,16 +16,11 @@ from collections.abc import Callable, Sequence from pathlib import Path -import time from typing import Protocol, TypeAlias, cast from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.manipulation.visualization.viser.animation import ( - GroupPreviewAnimation, - PreviewTrack, - sampled_joint_path_frames, -) +from dimos.manipulation.visualization.viser.animation import PreviewAnimator from dimos.manipulation.visualization.viser.runtime import ( VISER_INSTALL_HINT, VISER_URDF_INSTALL_HINT, @@ -36,7 +31,6 @@ try: from viser import ( - FrameHandle, GridHandle, MeshHandle, TransformControlsEvent, @@ -75,7 +69,7 @@ REFERENCE_GRID_CELL_COLOR = (44, 54, 58) REFERENCE_GRID_SECTION_COLOR = (90, 145, 165) -SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle | FrameHandle +SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle class _ColorHandle(Protocol): @@ -94,11 +88,9 @@ def __init__( self._configs_by_id: dict[str, RobotModelConfig] = {} self._urdfs: dict[str, ViserUrdf] = {} self._handles: dict[str, TransformControlsHandle] = {} - self._root_frames: dict[str, FrameHandle] = {} self._grid_handle: GridHandle | None = None self._grid_visible = True self._preview_visible: dict[str, bool] = {} - self._target_active: dict[str, bool] = {} self._target_tracks_current: dict[str, bool] = {} self._ensure_reference_grid() @@ -114,17 +106,9 @@ def set_reference_grid_visible(self, visible: bool) -> None: def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: self._configs_by_id[robot_id] = config self._preview_visible.setdefault(robot_id, False) - self._target_active.setdefault(robot_id, False) self._target_tracks_current.setdefault(robot_id, True) self._ensure_robot_urdfs(robot_id, config) - def set_target_active(self, robot_id: str, active: bool) -> None: - """Show target ghost only when at least one group on the robot is active.""" - self._target_active[robot_id] = active - if not active: - self._target_tracks_current[robot_id] = True - self._set_target_visibility(robot_id, active) - def _ensure_reference_grid(self) -> None: try: scene = self.server.scene @@ -171,9 +155,6 @@ def dispatch(event: TransformControlsEvent) -> None: self._handles[handle_key] = handle return handle - def remove_target_controls(self, robot_id: str) -> None: - self._remove_handle(f"{robot_id}:ee_control") - def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> None: config = self._configs_by_id.get(robot_id) if config is None or joint_state is None: @@ -183,7 +164,7 @@ def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> self.set_urdf_joints(current, config.joint_names, joint_state.position) if self._target_tracks_current.get(robot_id, True): self._set_target_joints(robot_id, config.joint_names, joint_state.position) - self._set_target_visibility(robot_id, self._target_active.get(robot_id, False)) + self._set_target_visibility(robot_id, True) def show_preview(self, robot_id: str) -> None: """Show the transient preview-animation ghost. @@ -202,63 +183,13 @@ def animate_path(self, robot_id: str, path: Sequence[JointState], duration: floa config = self._configs_by_id.get(robot_id) if config is None: return False - preview = GroupPreviewAnimation( - group_ids=(), - tracks=( - PreviewTrack( - robot_id=robot_id, - group_ids=(), - joint_names=tuple(config.joint_names), - path=tuple(path), - ), - ), - ) - return self.animate_preview(preview, duration) - - def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> bool: - """Animate all preview tracks with one shared group-native frame clock.""" - if not preview.tracks: - return False - frames_by_robot: dict[str, list[list[float]]] = {} - joint_names_by_robot: dict[str, tuple[str, ...]] = {} - for track in preview.tracks: - if track.robot_id not in self._configs_by_id: - return False - frames = sampled_joint_path_frames(track.path, duration, self.preview_fps) - if not frames: - return False - frames_by_robot[track.robot_id] = frames - joint_names_by_robot[track.robot_id] = track.joint_names - - frame_count = max(len(frames) for frames in frames_by_robot.values()) - if frame_count <= 0: - return False - step_delay = duration / max(frame_count - 1, 1) if duration > 0.0 else 0.0 - - robot_ids = tuple(frames_by_robot) - for robot_id in robot_ids: - self.show_preview(robot_id) + self.show_preview(robot_id) try: - for frame_index in range(frame_count): - for robot_id in robot_ids: - frames = frames_by_robot[robot_id] - joints = self._frame_at_shared_index(frames, frame_index, frame_count) - self._set_preview_ghost_joints(robot_id, joint_names_by_robot[robot_id], joints) - if frame_index < frame_count - 1: - time.sleep(step_delay) - return True + return PreviewAnimator( + lambda joints: self._set_preview_ghost_joints(robot_id, config.joint_names, joints) + ).animate(path, duration, self.preview_fps) finally: - for robot_id in robot_ids: - self.hide_preview(robot_id) - - @staticmethod - def _frame_at_shared_index( - frames: Sequence[list[float]], frame_index: int, frame_count: int - ) -> list[float]: - if frame_count <= 1 or len(frames) == 1: - return frames[-1] - source_index = round(frame_index * (len(frames) - 1) / (frame_count - 1)) - return frames[source_index] + self.hide_preview(robot_id) def set_target_joints( self, robot_id: str, joint_names: Sequence[str], joints: Sequence[float] @@ -266,7 +197,6 @@ def set_target_joints( target = self._urdfs.get(f"{robot_id}:target") if target is None: return False - self._target_active[robot_id] = True self._target_tracks_current[robot_id] = False self._set_target_joints(robot_id, joint_names, joints) self._set_target_visibility(robot_id, True) @@ -322,13 +252,9 @@ def close(self) -> None: self._grid_handle = None for urdf in self._urdfs.values(): self._remove_scene_handle(urdf) - for frame in self._root_frames.values(): - self._remove_scene_handle(frame) self._urdfs.clear() - self._root_frames.clear() self._configs_by_id.clear() self._preview_visible.clear() - self._target_active.clear() self._target_tracks_current.clear() def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: @@ -338,7 +264,11 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: key = f"{robot_id}:{kind}" if key in self._urdfs: continue - root_node_name = self._urdf_root_node_name(robot_id, kind, config) + root_node_name = { + "current": f"/robots/{robot_id}/current", + "target": f"/targets/{robot_id}/target", + "preview": f"/previews/{robot_id}/ghost", + }[kind] mesh_color_override = { "current": None, "target": GOAL_ROBOT_MESH_COLOR, @@ -354,9 +284,7 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: self._set_urdf_mesh_material( self._urdfs[key], GOAL_ROBOT_FEASIBLE_COLOR, GOAL_ROBOT_FEASIBLE_OPACITY ) - self._set_handle_visibility( - self._urdfs[key], self._target_active.get(robot_id, False) - ) + self._set_handle_visibility(self._urdfs[key], True) elif kind == "preview": self._set_urdf_mesh_material( self._urdfs[key], PREVIEW_ROBOT_COLOR, PREVIEW_ROBOT_OPACITY @@ -373,64 +301,6 @@ def prepared_urdf_path(self, config: RobotModelConfig) -> Path: package_paths=package_paths, xacro_args={str(key): str(value) for key, value in config.xacro_args.items()}, convert_meshes=bool(config.auto_convert_meshes), - strip_world_joint_child_link=str(config.base_link) - if bool(getattr(config, "strip_model_world_joint", False)) - else None, - ) - ) - - def _urdf_root_node_name(self, robot_id: str, kind: str, config: RobotModelConfig) -> str: - root_node_name = { - "current": f"/robots/{robot_id}/current", - "target": f"/targets/{robot_id}/target", - "preview": f"/previews/{robot_id}/ghost", - }[kind] - if not self._has_non_identity_base_pose(config): - return root_node_name - self._ensure_base_pose_frame(robot_id, kind, config) - return f"{root_node_name}/base_pose/urdf" - - def _ensure_base_pose_frame(self, robot_id: str, kind: str, config: RobotModelConfig) -> None: - key = f"{robot_id}:{kind}:base_pose" - if key in self._root_frames: - return - pose = config.base_pose - frame_name = { - "current": f"/robots/{robot_id}/current/base_pose", - "target": f"/targets/{robot_id}/target/base_pose", - "preview": f"/previews/{robot_id}/ghost/base_pose", - }[kind] - self._root_frames[key] = self.server.scene.add_frame( - frame_name, - show_axes=False, - position=( - float(pose.position.x), - float(pose.position.y), - float(pose.position.z), - ), - wxyz=( - float(pose.orientation.w), - float(pose.orientation.x), - float(pose.orientation.y), - float(pose.orientation.z), - ), - ) - - @staticmethod - def _has_non_identity_base_pose(config: RobotModelConfig) -> bool: - pose = getattr(config, "base_pose", None) - if pose is None: - return False - return any( - abs(value) > 1e-12 - for value in ( - float(pose.position.x), - float(pose.position.y), - float(pose.position.z), - float(pose.orientation.x), - float(pose.orientation.y), - float(pose.orientation.z), - float(pose.orientation.w) - 1.0, ) ) diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index f16e490ffb..b46097df95 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -21,8 +21,7 @@ import threading from typing import Literal -from dimos.manipulation.planning.spec.models import PlanningGroupID -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -93,25 +92,16 @@ class FeasibilityState: @dataclass class PanelPlanState: status: PlanStatus = PlanStatus.NONE - group_ids: tuple[PlanningGroupID, ...] = () robot: str | None = None target_pose: Pose | None = None target_joints: list[float] | None = None - start_joints_snapshot: dict[PlanningGroupID, list[float]] = field(default_factory=dict) + start_joints_snapshot: list[float] | None = None planned_path: list[JointState] | None = None @dataclass class PanelState: selected_robot: str | None = None - selected_group_ids: tuple[PlanningGroupID, ...] = () - auxiliary_group_ids: tuple[PlanningGroupID, ...] = () - pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) - group_joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) - target_joints: JointState | None = None - last_valid_target_joints: JointState | None = None - group_poses: dict[PlanningGroupID, Pose] = field(default_factory=dict) - group_diagnostics: dict[PlanningGroupID, str] = field(default_factory=dict) runtime: PanelRuntime = PanelRuntime.STOPPED backend_status: BackendConnectionStatus = BackendConnectionStatus.DISCONNECTED target_status: TargetStatus = TargetStatus.EMPTY @@ -143,10 +133,9 @@ def can_plan(self) -> bool: return ( self.runtime == PanelRuntime.RUNNING and self.backend_status == BackendConnectionStatus.READY - and bool(self.selected_group_ids) + and self.selected_robot is not None and self.action_status == ActionStatus.IDLE and self.target_status == TargetStatus.FEASIBLE - and self.target_joints is not None and self.manipulation_state in {"IDLE", "COMPLETED", "FAULT"} and self.plan_state.status != PlanStatus.PLANNING ) @@ -180,24 +169,19 @@ def can_execute( and self.target_status == TargetStatus.FEASIBLE and self.manipulation_state in {"IDLE", "COMPLETED"} and plan.status == PlanStatus.FRESH - and plan.group_ids == self.selected_group_ids - and bool(plan.start_joints_snapshot) - and self.target_joints is not None + and plan.robot == self.selected_robot + and plan.start_joints_snapshot is not None + and self.current_joints is not None ): return False - if self.current_joints is None: + if len(plan.start_joints_snapshot) != len(self.current_joints): return False - # Multi-group freshness is checked by group snapshot when available. The - # robot-level current-joint fallback preserves one-group legacy tests. - if len(plan.start_joints_snapshot) == 1: - snapshot = next(iter(plan.start_joints_snapshot.values())) - if len(snapshot) != len(self.current_joints): - return False - return all( - abs(expected - current) <= current_tolerance - for expected, current in zip(snapshot, self.current_joints, strict=False) + return all( + abs(expected - current) <= current_tolerance + for expected, current in zip( + plan.start_joints_snapshot, self.current_joints, strict=False ) - return True + ) @property def connected(self) -> bool: @@ -219,14 +203,9 @@ def module_state(self) -> str: class TargetEvaluationRequest: sequence_id: int source: PreviewSource - group_ids: tuple[PlanningGroupID, ...] - robot_name: str | None = None - auxiliary_group_ids: tuple[PlanningGroupID, ...] = () + robot_name: str pose: Pose | None = None - pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) joints: JointState | None = None - joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) - check_collision: bool = True class TargetEvaluationWorker: @@ -239,10 +218,8 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest], TargetEvaluation | TargetSetEvaluation], - apply_result: Callable[ - [TargetEvaluationRequest, TargetEvaluation | TargetSetEvaluation], None - ], + handler: Callable[[TargetEvaluationRequest], TargetEvaluation], + apply_result: Callable[[TargetEvaluationRequest, TargetEvaluation], None], ) -> None: self._handler = handler self._apply_result = apply_result diff --git a/dimos/manipulation/visualization/viser/test_gui_status.py b/dimos/manipulation/visualization/viser/test_gui_status.py index 617f32a988..f461565bdf 100644 --- a/dimos/manipulation/visualization/viser/test_gui_status.py +++ b/dimos/manipulation/visualization/viser/test_gui_status.py @@ -18,28 +18,46 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.visualization.viser.panel_backend import feasibility_status +from dimos.manipulation.visualization.types import TargetEvaluation +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig +from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.state import FeasibilityStatus +class StatusOnlyServer: + pass + + +class StatusOnlyAdapter(InProcessViserAdapter): + def __init__(self) -> None: + pass + + @pytest.mark.parametrize( - ("status", "success", "collision_free", "expected"), + ("result", "success", "collision_free", "expected"), [ - ("FEASIBLE", True, True, FeasibilityStatus.FEASIBLE), - ("COLLISION", False, False, FeasibilityStatus.COLLISION), - ("COLLISION_AT_START", False, False, FeasibilityStatus.COLLISION), - ("COLLISION_AT_GOAL", False, False, FeasibilityStatus.COLLISION), - ("NO_SOLUTION", False, False, FeasibilityStatus.IK_FAILED), - ("SINGULARITY", False, False, FeasibilityStatus.IK_FAILED), - ("JOINT_LIMITS", False, False, FeasibilityStatus.IK_FAILED), - ("TIMEOUT", False, False, FeasibilityStatus.IK_FAILED), - ("IK_SUCCEEDED", False, False, FeasibilityStatus.INVALID), + ({"status": "FEASIBLE"}, True, True, FeasibilityStatus.FEASIBLE), + ({"status": "COLLISION"}, False, False, FeasibilityStatus.COLLISION), + ({"status": "COLLISION_AT_START"}, False, False, FeasibilityStatus.COLLISION), + ({"status": "COLLISION_AT_GOAL"}, False, False, FeasibilityStatus.COLLISION), + ({"status": "NO_SOLUTION"}, False, False, FeasibilityStatus.IK_FAILED), + ({"status": "SINGULARITY"}, False, False, FeasibilityStatus.IK_FAILED), + ({"status": "JOINT_LIMITS"}, False, False, FeasibilityStatus.IK_FAILED), + ({"status": "TIMEOUT"}, False, False, FeasibilityStatus.IK_FAILED), + ({"status": "IK_SUCCEEDED"}, False, False, FeasibilityStatus.INVALID), ], ) def test_gui_feasibility_status_uses_exact_status_mapping( - status: str, + result: TargetEvaluation, success: bool, collision_free: bool, expected: FeasibilityStatus, ) -> None: - assert feasibility_status(status, success, collision_free) == expected + gui = ViserPanelGui( + StatusOnlyServer(), + StatusOnlyAdapter(), + ViserVisualizationConfig(), + ) + + assert gui._feasibility_status(result, success, collision_free) == expected diff --git a/dimos/manipulation/visualization/viser/test_operation_worker.py b/dimos/manipulation/visualization/viser/test_operation_worker.py index 8367d05a19..ee1d5c29c0 100644 --- a/dimos/manipulation/visualization/viser/test_operation_worker.py +++ b/dimos/manipulation/visualization/viser/test_operation_worker.py @@ -22,8 +22,8 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.visualization.types import RobotInfo +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.state import ( @@ -42,14 +42,6 @@ class EmptyServer: pass -class EmptyWorldMonitor: - def get_current_joint_state(self, robot_id: str) -> None: - return None - - def is_state_stale(self, robot_id: str, max_age: float = 1.0) -> bool: - return False - - @dataclass class FakeStopOperationWorker(OperationWorker): stop_calls: list[float | None] @@ -114,25 +106,19 @@ def stop(self, timeout: float | None = 2.0) -> None: self.stop_calls.append(timeout) -class FakeOperationAdapter: +class FakeOperationAdapter(InProcessViserAdapter): def __init__(self) -> None: self.cancel_calls = 0 def list_robots(self) -> list[str]: return [] - def robot_id_for_name(self, robot_name: str) -> str | None: - return None - - def get_state(self) -> str: + def get_module_state(self) -> str: return "IDLE" def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None - def list_planning_groups(self) -> list[PlanningGroup]: - return [] - def get_current_joint_state(self, robot_name: str) -> None: return None @@ -155,12 +141,6 @@ def cancel(self) -> bool: def plan_to_joints(self, joints: JointState, robot_name: str | None = None) -> bool: return True - def robot_items(self) -> list[tuple[str, str, object]]: - return [] - - def plan_to_joint_targets(self, joint_targets: dict[str, JointState]) -> bool: - return True - def test_operation_worker_uses_per_operation_timeout() -> None: errors: list[str] = [] @@ -201,7 +181,6 @@ def test_gui_close_uses_bounded_operation_worker_stop(monkeypatch: pytest.Monkey stop_timeouts: list[float | None] = [] gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) @@ -219,7 +198,6 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat submissions: list[dict[str, float]] = [] gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(preview_request_timeout=0.25), ) @@ -227,8 +205,7 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat monkeypatch.setattr(gui, "_operation_worker", FakeTimeoutSubmitWorker(submissions)) gui.state.runtime = PanelRuntime.RUNNING gui.state.backend_status = BackendConnectionStatus.READY - gui.state.selected_group_ids = ("arm:manipulator",) - gui.state.target_joints = JointState({"name": ["arm/j1"], "position": [1.0]}) + gui.state.selected_robot = "arm" gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -246,7 +223,6 @@ def test_gui_cancel_bypasses_operation_worker(monkeypatch: pytest.MonkeyPatch) - adapter = FakeOperationAdapter() gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), adapter, ViserVisualizationConfig(), ) @@ -272,7 +248,6 @@ def test_gui_cancelled_planning_clears_active_plan_state(monkeypatch: pytest.Mon adapter = FakeOperationAdapter() gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), adapter, ViserVisualizationConfig(), ) @@ -311,7 +286,6 @@ def test_gui_guard_errors_keep_action_idle( submissions: list[Callable[[], None]] = [] gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) @@ -332,7 +306,6 @@ def test_gui_guard_errors_keep_action_idle( def test_gui_ignores_stale_timed_out_operation_finish() -> None: gui = ViserPanelGui( EmptyServer(), - EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index 040ae6f56e..412c227050 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -20,14 +20,11 @@ PanelState, TargetStatus, ) -from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_can_plan_from_fault_after_planning_failure() -> None: state = PanelState( selected_robot="arm", - selected_group_ids=("arm:manipulator",), - target_joints=JointState({"name": ["arm/j1"], "position": [1.0]}), runtime=PanelRuntime.RUNNING, backend_status=BackendConnectionStatus.READY, target_status=TargetStatus.FEASIBLE, diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 42d78c068f..3be6162a67 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -25,30 +25,15 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import ( - CollisionCheckResult, - ForwardKinematicsResult, - IKResult, -) -from dimos.manipulation.visualization.types import RobotInfo -from dimos.manipulation.visualization.viser import scene as scene_module +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.animation import ( - GroupPreviewAnimation, PreviewAnimator, - PreviewTrack, interpolate_joint_path, sampled_joint_path_frames, ) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.gui import ( - ACTIVE_GROUP_COLOR, - INACTIVE_GROUP_COLOR, - PRIMARY_ACTION_COLOR, - ViserPanelGui, -) -from dimos.manipulation.visualization.viser.panel_backend import pose_from_transform_values +from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( ActionStatus, @@ -62,15 +47,11 @@ ) from dimos.manipulation.visualization.viser.theme import _dimos_logo_data_url, apply_dimos_theme from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState GuiCallback = Callable[[SimpleNamespace], None] ThemeValue = str | bool | tuple[int, int, int] | dict[str, str | dict[str, str]] | None RobotConfigOverride = str | list[str] | list[float] | None -DEFAULT_GROUP_ID = "arm:manipulator" @dataclass @@ -139,7 +120,6 @@ def remove(self) -> None: class GuiButtonHandle: label: str disabled: bool = False - color: tuple[int, int, int] | None = None click_callback: GuiCallback | None = None removed: bool = False @@ -186,7 +166,7 @@ def __init__(self) -> None: self.visible: object | None = None self.removed = False self.name = "" - self.kwargs: dict[str, object] = {} + self.kwargs: dict[str, float | bool] = {} def remove(self) -> None: self.removed = True @@ -226,8 +206,6 @@ class FakeServer: def __init__(self) -> None: self.scene = SimpleNamespace() self.scene.add_transform_controls = self.add_transform_controls - self.scene.add_frame = self.add_frame - self.frames = [] def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHandle: handle = FakeTransformHandle() @@ -235,13 +213,6 @@ def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHan handle.scale = scale return handle - def add_frame(self, name: str, **kwargs: object) -> FakeHandle: - handle = FakeHandle() - handle.name = name - handle.kwargs = kwargs - self.frames.append(handle) - return handle - class FakeGridServer(FakeServer): def __init__(self) -> None: @@ -252,7 +223,7 @@ def __init__(self) -> None: def add_grid(self, name: str, **kwargs: float | bool) -> FakeHandle: handle = FakeHandle() handle.name = name - handle.kwargs = dict(kwargs) + handle.kwargs = kwargs handle.visible = kwargs.get("visible") self.grids.append(handle) return handle @@ -343,16 +314,8 @@ def add_dropdown( handle = GuiDropdownHandle(label=label, options=list(options), value=initial_value) return handle - def add_button( - self, - label: str, - *, - disabled: bool = False, - color: tuple[int, int, int] | None = None, - hint: str | None = None, - ) -> GuiButtonHandle: - _ = hint - handle = GuiButtonHandle(label=label, disabled=disabled, color=color) + def add_button(self, label: str, *, disabled: bool = False) -> GuiButtonHandle: + handle = GuiButtonHandle(label=label, disabled=disabled) self.buttons[label] = handle return handle @@ -383,28 +346,8 @@ def make_robot_config(**overrides: RobotConfigOverride) -> RobotConfigStub: return config -def make_planning_group_info( - robot_name: str, - config: RobotConfigStub | SimpleNamespace, - *, - group_name: str = "manipulator", - has_pose_target: bool = True, -) -> PlanningGroup: - joint_names = [str(name) for name in config.joint_names] - return PlanningGroup( - id=f"{robot_name}:{group_name}", - group_name=group_name, - robot_name=robot_name, - joint_names=tuple(f"{robot_name}/{name}" for name in joint_names), - local_joint_names=tuple(joint_names), - base_link=str(config.base_link), - tip_link=str(config.end_effector_link) if has_pose_target else None, - source="fallback", - ) - - class FakeManipulationModule(SimpleNamespace): - """Public ManipulationModule surface used by the in-process Viser panel tests.""" + """Public ManipulationModule surface used by the in-process Viser adapter tests.""" def list_robots(self) -> list[str]: return list(getattr(self, "_robots", {}).keys()) @@ -435,16 +378,10 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None init = self.get_init_joints(robot_name) home_joints = config.home_joints if hasattr(config, "home_joints") else None - planning_groups = getattr(self, "_planning_groups", None) - if planning_groups is None: - planning_groups = [make_planning_group_info(robot_name, config)] - else: - planning_groups = [group for group in planning_groups if group.robot_name == robot_name] return { "name": config.name, "world_robot_id": self.robot_id_for_name(robot_name) or robot_name, "joint_names": list(config.joint_names), - "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": 1.0, @@ -456,17 +393,16 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: "init_joints": list(init.position) if init is not None else None, } - def list_planning_groups(self) -> list[PlanningGroup]: - groups: list[PlanningGroup] = [] - for robot_name in self.list_robots(): - info = self.get_robot_info(robot_name) - if info is not None: - groups.extend(info.get("planning_groups", [])) - return groups - def get_init_joints(self, robot_name: str) -> JointState | None: return getattr(self, "_init_joints", {}).get(robot_name) + def get_planned_path(self, robot_name: str) -> list[JointState] | None: + return getattr(self, "_planned_paths", {}).get(robot_name) + + def get_planned_trajectory_duration(self, robot_name: str) -> float | None: + trajectory = getattr(self, "_planned_trajectories", {}).get(robot_name) + return None if trajectory is None else float(trajectory.duration) + def get_state(self) -> str: state = getattr(self, "_state", "IDLE") return str(getattr(state, "name", state)) @@ -474,100 +410,34 @@ def get_state(self) -> str: def get_error(self) -> str: return str(getattr(self, "_error_message", "")) - def get_current_joint_state(self, robot_name: str) -> JointState | None: - robot_id = self.robot_id_for_name(robot_name) - world_monitor = getattr(self, "_world_monitor", None) - if robot_id is None or world_monitor is None: - return None - return world_monitor.get_current_joint_state(robot_id) - - def check_collision( - self, target_joints: JointState, max_age: float = 1.0 - ) -> CollisionCheckResult: - del max_age - world_monitor = getattr(self, "_world_monitor", None) - if world_monitor is not None and hasattr(world_monitor, "check_collision"): - return world_monitor.check_collision(target_joints) - collision_free = True - if world_monitor is not None: - for robot_name in self.list_robots(): - robot_id = self.robot_id_for_name(robot_name) - if robot_id is not None: - collision_free = collision_free and world_monitor.is_state_valid( - robot_id, target_joints - ) - return CollisionCheckResult( - status="VALID" if collision_free else "COLLISION", - collision_free=collision_free, - message="Target is collision-free" if collision_free else "Target is in collision", - ) - - def forward_kinematics( - self, - group_id: str, - target_joints: JointState | None = None, - max_age: float = 1.0, - ) -> ForwardKinematicsResult: - del max_age - robot_name = group_id.split(":", 1)[0].split("/", 1)[0] + def evaluate_joint_target(self, joints: JointState | None, robot_name: str) -> TargetEvaluation: robot_id = self.robot_id_for_name(robot_name) + if robot_id is None or joints is None: + return {"success": False, "status": "NO_ROBOT", "joint_state": None} world_monitor = getattr(self, "_world_monitor", None) - pose = None - if world_monitor is not None and robot_id is not None: - if hasattr(world_monitor, "get_group_ee_pose"): - pose = world_monitor.get_group_ee_pose(group_id, target_joints) - else: - pose = world_monitor.get_ee_pose(robot_id, target_joints) - return ForwardKinematicsResult( - status="VALID", pose=pose, message="Forward kinematics solved" - ) - - def _current_values_by_name(self, robot_name: str) -> dict[str, float]: - current = self.get_current_joint_state(robot_name) - if current is None: - return {} - values: dict[str, float] = {} - for name, position in zip(current.name, current.position, strict=False): - name_str = str(name) - values[name_str] = float(position) - if "/" in name_str: - values[name_str.rsplit("/", 1)[1]] = float(position) - else: - values[f"{robot_name}/{name_str}"] = float(position) - return values - - def inverse_kinematics( - self, - pose_targets: dict[str, PoseStamped], - auxiliary_group_ids: Sequence[str] = (), - seed: JointState | None = None, - ) -> IKResult: - del seed - group_ids = tuple(pose_targets) + tuple(auxiliary_group_ids) - names: list[str] = [] - positions: list[float] = [] - for group_id in group_ids or (DEFAULT_GROUP_ID,): - robot_name = group_id.split(":", 1)[0].split("/", 1)[0] - config = self.get_robot_config(robot_name) - if config is None: - continue - for joint_name in config.joint_names: - names.append(f"{robot_name}/{joint_name}") - positions.append(0.1 + 0.1 * len(positions)) - return IKResult( - status=IKStatus.SUCCESS, - joint_state=JointState({"name": names, "position": positions}), - message="Target is collision-free", - ) - - def plan_to_joint_targets(self, _joint_targets: dict[str, JointState]) -> bool: - return True + if world_monitor is None: + return {"success": False, "status": "UNAVAILABLE", "joint_state": None} + collision_free = world_monitor.is_state_valid(robot_id, joints) + return { + "success": True, + "status": "FEASIBLE" if collision_free else "COLLISION", + "message": "Target is collision-free" if collision_free else "Target is in collision", + "collision_free": collision_free, + "ee_pose": world_monitor.get_ee_pose(robot_id, joints), + "joint_state": joints, + } - def reset(self) -> bool: - return True + def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluation: + return { + "success": False, + "joint_state": None, + "status": "UNAVAILABLE", + "message": "No fake pose IK", + "collision_free": False, + } -def make_module_with_robot() -> tuple[SimpleNamespace, FakeManipulationModule]: +def make_adapter_with_robot() -> InProcessViserAdapter: current = FakeJointState(["j1", "j2"], position=[0.3, 0.4]) config = make_robot_config( name="arm", @@ -585,15 +455,16 @@ def make_module_with_robot() -> tuple[SimpleNamespace, FakeManipulationModule]: module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[0.1, 0.2])}, + _planned_paths={}, + _planned_trajectories={}, _state=NamedState(name="IDLE"), _error_message="", _world_monitor=world_monitor, ) - return world_monitor, module - - -def joints_from_values(joint_names: Sequence[str], values: Sequence[float]) -> JointState: - return JointState({"name": list(joint_names), "position": [float(value) for value in values]}) + return InProcessViserAdapter( + world_monitor=world_monitor, + manipulation_module=module, + ) @pytest.fixture @@ -603,16 +474,12 @@ def make_panel() -> Iterator[Callable[..., ViserPanelGui]]: def _make( server: FakeGuiServer | FakeServer, - module_context: tuple[SimpleNamespace, FakeManipulationModule], + adapter: InProcessViserAdapter, config: ViserVisualizationConfig | None = None, scene: ViserManipulationScene | None = None, ) -> ViserPanelGui: gui = ViserPanelGui( - server, - module_context[0], - module_context[1], - config or ViserVisualizationConfig(panel_enabled=True), - scene, + server, adapter, config or ViserVisualizationConfig(panel_enabled=True), scene ) gui.start() panels.append(gui) @@ -631,50 +498,14 @@ def test_gui_builds_controls_in_manipulation_panel_folder( make_panel: Callable[..., ViserPanelGui], ) -> None: server = FakeGuiServer() - module_context = make_module_with_robot() - gui = make_panel(server, module_context, ViserVisualizationConfig()) + adapter = make_adapter_with_robot() + gui = make_panel(server, adapter, ViserVisualizationConfig()) assert server.folders assert server.folders[0].label == "Manipulation Panel" assert server.folders[0].kwargs == {"expand_by_default": True} assert "status" in gui._handles - assert "robot" not in gui._handles - assert "planning_groups_heading" in gui._handles - assert "target_heading" in gui._handles - assert "target_summary" in gui._handles - assert "actions_heading" in gui._handles + assert "robot" in gui._handles assert "plan" in gui._handles - assert "select_all_manipulators" not in gui._handles - assert "clear_group_selection" not in gui._handles - assert "plan_controls_heading" in gui._handles - assert "actions_folder" not in gui._handles - assert "joint_control_folder" in gui._handles - handle_order = list(gui._handles) - assert handle_order.index(f"group:{DEFAULT_GROUP_ID}") < handle_order.index("plan") - assert handle_order.index("target_summary") < handle_order.index("plan") - assert handle_order.index("plan") < handle_order.index("plan_controls_heading") - assert handle_order.index("plan_controls_heading") < handle_order.index("preview") - assert handle_order.index("preview") < handle_order.index("execute") - assert handle_order.index("clear") < handle_order.index("joint_control_folder") - assert isinstance(gui._handles["status"], GuiMarkdownHandle) - assert "Starting" not in gui._handles["status"].value - assert isinstance(gui._handles["target_summary"], GuiMarkdownHandle) - assert "Feasibility:" in gui._handles["target_summary"].value - assert "Primary:" not in gui._handles["target_summary"].value - assert "Auxiliary:" not in gui._handles["target_summary"].value - assert "Ghosts:" not in gui._handles["target_summary"].value - assert isinstance(gui._handles["plan_controls_heading"], GuiMarkdownHandle) - assert "Plan controls" in gui._handles["plan_controls_heading"].value - plan_button = gui._handles["plan"] - assert isinstance(plan_button, GuiButtonHandle) - assert plan_button.color == PRIMARY_ACTION_COLOR - group_button = gui._handles[f"group:{DEFAULT_GROUP_ID}"] - assert isinstance(group_button, GuiButtonHandle) - assert group_button.label == "arm" - assert group_button.color == ACTIVE_GROUP_COLOR - joint_folder = gui._handles["joint_control_folder"] - assert isinstance(joint_folder, FakeFolder) - assert joint_folder.label == "Joint Control" - assert joint_folder.kwargs == {"expand_by_default": False} assert gui._operation_worker._timeout_seconds is None @@ -686,8 +517,8 @@ def test_gui_scene_grid_checkbox_toggles_reference_grid( grid_server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 ) server = FakeGuiServer() - module_context = make_module_with_robot() - make_panel(server, module_context, ViserVisualizationConfig(), scene) + adapter = make_adapter_with_robot() + make_panel(server, adapter, ViserVisualizationConfig(), scene) assert grid_server.grids assert server.checkboxes["Scene grid"].value is True server.checkboxes["Scene grid"].update_callback( @@ -708,13 +539,16 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( scene = ViserManipulationScene( grid_server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 ) - module_context = make_module_with_robot() - gui = make_panel(server, module_context, ViserVisualizationConfig(), scene) + adapter = make_adapter_with_robot() + gui = make_panel(server, adapter, ViserVisualizationConfig(), scene) + robot_dropdown = gui._handles["robot"] plan_button = server.buttons["Plan"] grid = grid_server.grids[0] handles = list(gui._handles.values()) gui.close() + if isinstance(robot_dropdown, GuiDropdownHandle) and robot_dropdown.update_callback is not None: + robot_dropdown.update_callback(SimpleNamespace(target=SimpleNamespace(value="arm"))) if plan_button.click_callback is not None: plan_button.click_callback(SimpleNamespace()) gui._set_scene_grid_visible(False) @@ -727,15 +561,15 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( def test_gui_ignores_target_evaluation_after_close( make_panel: Callable[..., ViserPanelGui], ) -> None: - module_context = make_module_with_robot() - gui = make_panel(FakeGuiServer(), module_context) + adapter = make_adapter_with_robot() + gui = make_panel(FakeGuiServer(), adapter) gui.state.selected_robot = "arm" sequence_id = gui.state.next_sequence_id() request = TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - group_ids=(DEFAULT_GROUP_ID,), - joint_targets={DEFAULT_GROUP_ID: FakeJointState(["arm/j1", "arm/j2"], position=[0.1, 0.2])}, + robot_name="arm", + joints=FakeJointState(["j1", "j2"], position=[0.1, 0.2]), ) gui.close() @@ -758,12 +592,12 @@ def test_dimos_theme_configures_supported_viser_chrome() -> None: assert apply_dimos_theme(server) is True assert server.theme_kwargs is not None - assert server.theme_kwargs["brand_color"] == (0, 153, 255) + assert server.theme_kwargs["brand_color"] == (22, 130, 163) assert server.theme_kwargs["dark_mode"] is True assert server.theme_kwargs["show_logo"] is False assert server.theme_kwargs["show_share_button"] is False - assert server.theme_kwargs["control_layout"] == "fixed" - assert server.theme_kwargs["control_width"] == "large" + assert server.theme_kwargs["control_layout"] == "collapsible" + assert server.theme_kwargs["control_width"] == "medium" def test_dimos_theme_configures_titlebar_when_supported(monkeypatch: pytest.MonkeyPatch) -> None: @@ -903,10 +737,8 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles scene.register_robot("robot1", config) target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is False for mesh in target._meshes) - assert all(mesh.visible is False for mesh in preview._meshes) - scene.set_target_active("robot1", True) assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in preview._meshes) scene.show_preview("robot1") assert all(mesh.visible is True for mesh in preview._meshes) assert all(mesh.visible is True for mesh in target._meshes) @@ -918,7 +750,7 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles assert all(mesh.visible is False for mesh in preview._meshes) -def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: +def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> None: server = FakeServer() urdfs = [FakeViserUrdfWithMeshes(("joint1",)) for _ in range(3)] scene = ViserManipulationScene(server, lambda *args, **kwargs: urdfs.pop(0), preview_fps=10.0) @@ -936,16 +768,12 @@ def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is False for mesh in target._meshes) + assert all(mesh.visible is True for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.25])) assert current.cfg == [0.25] assert target.cfg == [0.25] assert preview.cfg is None - assert all(mesh.visible is False for mesh in target._meshes) - - scene.set_target_active("robot1", True) - assert all(mesh.visible is True for mesh in target._meshes) scene.set_target_joints("robot1", ["joint1"], [0.8]) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.1])) @@ -953,51 +781,6 @@ def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: assert target.cfg == [0.8] assert preview.cfg is None - scene.set_target_active("robot1", False) - assert all(mesh.visible is False for mesh in target._meshes) - - -def test_scene_parents_urdfs_under_base_pose_frame() -> None: - server = FakeServer() - root_node_names: list[str] = [] - - def make_urdf(*_: object, **kwargs: object) -> FakeViserUrdfWithMeshes: - root_node_names.append(str(kwargs["root_node_name"])) - return FakeViserUrdfWithMeshes(("joint1",)) - - scene = ViserManipulationScene(server, make_urdf, preview_fps=10.0) - scene.prepared_urdf_path = lambda config: "dummy.urdf" - config = SimpleNamespace( - name="arm", - model_path="/tmp/arm.urdf", - package_paths={}, - xacro_args={}, - auto_convert_meshes=False, - joint_names=["joint1"], - base_pose=PoseStamped( - position=Vector3(1.0, 2.0, 3.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - ) - - scene.register_robot("robot1", config) - - assert [frame.name for frame in server.frames] == [ - "/robots/robot1/current/base_pose", - "/targets/robot1/target/base_pose", - "/previews/robot1/ghost/base_pose", - ] - assert [frame.kwargs["position"] for frame in server.frames] == [ - (1.0, 2.0, 3.0), - (1.0, 2.0, 3.0), - (1.0, 2.0, 3.0), - ] - assert root_node_names == [ - "/robots/robot1/current/base_pose/urdf", - "/targets/robot1/target/base_pose/urdf", - "/previews/robot1/ghost/base_pose/urdf", - ] - def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback() -> None: server = FakeServer() @@ -1032,74 +815,7 @@ def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback( assert ok is True assert preview.cfg == [1.0] assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is False for mesh in target._meshes) - - -def test_group_preview_animation_updates_all_tracks_on_shared_frame_clock( - monkeypatch: pytest.MonkeyPatch, -) -> None: - server = FakeServer() - scene = ViserManipulationScene( - server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 - ) - scene.prepared_urdf_path = lambda config: "dummy.urdf" - config = SimpleNamespace( - name="arm", - model_path="/tmp/arm.urdf", - package_paths={}, - xacro_args={}, - auto_convert_meshes=False, - joint_names=["joint1"], - ) - scene.register_robot("left", config) - scene.register_robot("right", config) - updates: list[tuple[str, tuple[str, ...], tuple[float, ...]]] = [] - sleep_calls: list[float] = [] - - def record_preview_joints( - robot_id: str, joint_names: Sequence[str], joints: Sequence[float] - ) -> None: - updates.append((robot_id, tuple(joint_names), tuple(joints))) - - monkeypatch.setattr(scene, "_set_preview_ghost_joints", record_preview_joints) - monkeypatch.setattr(scene_module.time, "sleep", sleep_calls.append) - - ok = scene.animate_preview( - GroupPreviewAnimation( - group_ids=("left/arm", "right/arm"), - tracks=( - PreviewTrack( - robot_id="left", - group_ids=("left/arm",), - joint_names=("joint1",), - path=( - FakeJointState(["joint1"], position=[0.0]), - FakeJointState(["joint1"], position=[1.0]), - ), - ), - PreviewTrack( - robot_id="right", - group_ids=("right/arm",), - joint_names=("joint1",), - path=( - FakeJointState(["joint1"], position=[10.0]), - FakeJointState(["joint1"], position=[11.0]), - ), - ), - ), - ), - duration=0.0, - ) - - assert ok is True - assert updates == [ - ("left", ("joint1",), (0.0,)), - ("right", ("joint1",), (10.0,)), - ("left", ("joint1",), (1.0,)), - ("right", ("joint1",), (11.0,)), - ] - assert sleep_calls == [0.0] - assert scene._preview_visible == {"left": False, "right": False} + assert all(mesh.visible is True for mesh in target._meshes) def test_scene_target_helpers_handle_missing_robot_and_pose() -> None: @@ -1183,6 +899,105 @@ def test_joint_path_frame_edge_cases_and_empty_animation() -> None: assert sleep_calls == [] +def test_adapter_copies_joint_state_and_delegates_to_module() -> None: + copied = FakeJointState(["j1"], position=[1.0], velocity=[2.0], effort=[3.0]) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", SimpleNamespace(), None)}, + _planned_paths={"arm": [copied]}, + _planned_trajectories={}, + plan_to_pose=lambda pose, robot_name=None: (pose, robot_name), + plan_to_joints=lambda joints, robot_name=None: (joints, robot_name), + preview_path=lambda robot_name=None: robot_name, + execute=lambda robot_name=None: robot_name, + cancel=lambda: True, + clear_planned_path=lambda: True, + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: copied, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: (robot_id, joint_state), + ) + module._world_monitor = world_monitor + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + + planned = adapter.get_planned_path("arm") + assert planned is not None + assert planned[0] is not copied + assert planned[0].name is not copied.name + assert planned[0].position is not copied.position + + current = adapter.get_current_joint_state("arm") + assert current is not copied + assert current.name is not copied.name + + assert adapter.plan_to_pose("pose", "arm") == ("pose", "arm") + assert adapter.preview_path("arm") == "arm" + assert adapter.evaluate_joint_target(planned[0], "arm")["status"] == "FEASIBLE" + + +def test_adapter_evaluate_joint_target_uses_world_monitor_and_copies_input() -> None: + original = FakeJointState(["arm/j1", "j2"], position=[1.0, 2.0]) + seen = {} + + def is_state_valid(robot_id, joint_state) -> bool: + seen["robot_id"] = robot_id + seen["joint_state"] = joint_state + return True + + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: None, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=is_state_valid, + get_ee_pose=lambda robot_id, joint_state=None: (robot_id, joint_state), + ) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", SimpleNamespace(), None)}, + _world_monitor=world_monitor, + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + + result = adapter.evaluate_joint_target(original, "arm") + + assert result["success"] is True + assert result["status"] == "FEASIBLE" + assert seen["robot_id"] == "robot-1" + assert seen["joint_state"] is not original + assert seen["joint_state"].name == ["arm/j1", "j2"] + assert seen["joint_state"].position == [1.0, 2.0] + + +def test_obstacle_collision_marks_joint_target_infeasible() -> None: + obstacle = SimpleNamespace(name="blocking_box", blocked_joint_min=0.5) + + def is_state_valid(robot_id, joint_state) -> bool: + return bool(joint_state.position[0] < obstacle.blocked_joint_min) + + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: FakeJointState(["j1"], position=[0.0]), + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=is_state_valid, + get_ee_pose=lambda robot_id, joint_state=None: SimpleNamespace( + position=SimpleNamespace(x=0.0, y=0.0, z=0.0) + ), + ) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", SimpleNamespace(joint_names=["j1"]), None)}, + _world_monitor=world_monitor, + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + + free = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[0.25]), "arm") + colliding = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[0.75]), "arm") + + assert free["success"] is True + assert free["status"] == "FEASIBLE" + assert free["collision_free"] is True + assert colliding["success"] is True + assert colliding["status"] == "COLLISION" + assert colliding["collision_free"] is False + + def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: server = FakeServer() scene = ViserManipulationScene( @@ -1213,7 +1028,7 @@ def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: assert all(mesh.visible is True for mesh in preview._meshes) scene.hide_preview("robot1") assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is False for mesh in target._meshes) + assert all(mesh.visible is True for mesh in target._meshes) def test_scene_transform_controls_update_pose_callback_and_visual_state() -> None: @@ -1297,153 +1112,25 @@ def test_gui_initializes_pose_selector_to_current_ee_pose( orientation=SimpleNamespace(w=0.9, x=0.1, y=0.2, z=0.3), ) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} + ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, get_ee_pose=lambda robot_id, joint_state=None: current_pose, ) - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) scene = ViserManipulationScene( FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + control = scene._handles["robot-1:ee_control"] assert control.position == (0.1, 0.2, 0.3) assert control.wxyz == (0.9, 0.1, 0.2, 0.3) assert gui.state.cartesian_target is current_pose -def test_gui_removes_pose_selector_when_group_is_deselected( - make_panel: Callable[..., ViserPanelGui], -) -> None: - current = FakeJointState(["j1"], position=[0.25]) - current_pose = SimpleNamespace( - position=SimpleNamespace(x=0.1, y=0.2, z=0.3), - orientation=SimpleNamespace(w=1.0, x=0.0, y=0.0, z=0.0), - ) - config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: current, - is_state_stale=lambda robot_id, max_age=1.0: False, - get_ee_pose=lambda robot_id, joint_state=None: current_pose, - ) - module_context = (world_monitor, module) - scene = ViserManipulationScene( - FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 - ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] - - gui._set_group_selected(DEFAULT_GROUP_ID, False) - - assert f"{DEFAULT_GROUP_ID}:ee_control" not in scene._handles - assert control.removed is True - - -def test_gui_group_selector_derives_primary_and_auxiliary_groups( - make_panel: Callable[..., ViserPanelGui], -) -> None: - current = FakeJointState(["j1", "grip"], position=[0.25, 0.5]) - config = make_robot_config(joint_names=["j1", "grip"], home_joints=[0.0, 0.0]) - pose_group = make_planning_group_info("arm", config) - auxiliary_group = make_planning_group_info( - "arm", config, group_name="gripper", has_pose_target=False - ) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, - _planning_groups=[pose_group, auxiliary_group], - ) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: current, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=lambda robot_id, joint_state: True, - get_ee_pose=lambda robot_id, joint_state=None: Pose( - {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} - ), - ) - module_context = (world_monitor, module) - target_controls = [] - scene = SimpleNamespace( - has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: target_controls.append(args) or object(), - remove_target_controls=lambda *args: None, - set_target_active=lambda *args: None, - set_target_joints=lambda *args: True, - set_target_pose=lambda *args: None, - set_target_visual_state=lambda *args: None, - ) - server = FakeGuiServer() - - gui = make_panel(server, module_context, ViserVisualizationConfig(panel_enabled=True), scene) - assert "robot" not in gui._handles - pose_button = gui._handles["group:arm:manipulator"] - aux_button = gui._handles["group:arm:gripper"] - assert isinstance(pose_button, GuiButtonHandle) - assert isinstance(aux_button, GuiButtonHandle) - assert pose_button.label == "arm" - assert pose_button.color == ACTIVE_GROUP_COLOR - assert aux_button.label == "arm gripper" - assert aux_button.color == INACTIVE_GROUP_COLOR - - assert aux_button.click_callback is not None - aux_button.click_callback(SimpleNamespace(target=aux_button)) - - assert gui.state.selected_group_ids == ("arm:manipulator", "arm:gripper") - assert gui.state.auxiliary_group_ids == ("arm:gripper",) - assert aux_button.label == "arm gripper" - assert aux_button.color == ACTIVE_GROUP_COLOR - assert [call[0] for call in target_controls] == ["arm:manipulator"] - - -def test_gui_target_ghost_visibility_follows_active_selected_groups( - make_panel: Callable[..., ViserPanelGui], -) -> None: - left_config = make_robot_config(name="left", joint_names=["j1"], home_joints=[0.0]) - right_config = make_robot_config(name="right", joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule( - _robots={ - "left": ("left-id", left_config, None), - "right": ("right-id", right_config, None), - } - ) - current = FakeJointState(["j1"], position=[0.0]) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: current, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=lambda robot_id, joint_state: True, - get_ee_pose=lambda robot_id, joint_state=None: Pose( - {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} - ), - ) - module_context = (world_monitor, module) - active_updates = [] - scene = SimpleNamespace( - has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: object(), - remove_target_controls=lambda *args: None, - set_target_active=lambda *args: active_updates.append(args), - set_target_joints=lambda *args: True, - set_target_pose=lambda *args: None, - set_target_visual_state=lambda *args: None, - ) - - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - - assert active_updates[-2:] == [("left-id", True), ("right-id", False)] - gui._set_group_selected("right:manipulator", True) - assert active_updates[-2:] == [("left-id", True), ("right-id", True)] - gui._set_group_selected("left:manipulator", False) - assert active_updates[-2:] == [("left-id", False), ("right-id", True)] - - def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callbacks( make_panel: Callable[..., ViserPanelGui], ) -> None: @@ -1452,6 +1139,8 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[-1.0, -2.0])}, + _planned_paths={}, + _planned_trajectories={}, ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, @@ -1459,14 +1148,14 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - module_context = (world_monitor, module) - gui = make_panel(FakeGuiServer(), module_context) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + gui = make_panel(FakeGuiServer(), adapter) assert gui._handles["preset"].options == ["Select preset...", "Init", "Current", "Home"] - assert list(gui._joint_sliders) == ["arm/j1", "arm/j2"] + assert list(gui._joint_sliders) == ["j1", "j2"] gui._apply_preset("Home") - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] gui._apply_preset("Current") - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.5] + assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.5] gui._submit_execute() assert gui.state.error == "Panel execution disabled; set allow_plan_execute=True to enable" @@ -1476,37 +1165,42 @@ def test_gui_rebuilding_joint_sliders_removes_stale_viser_handles( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[1.0, 2.0]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} + ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) server = FakeGuiServer() - gui = make_panel(server, module_context) + gui = make_panel(server, adapter) stale_sliders = list(server.sliders) assert [slider.value for slider in stale_sliders] == [0.0, 0.0] current.position = [-0.738, -0.2826151825863572] - gui.state.target_joints = None - gui.state.group_joint_targets.clear() gui._build_joint_sliders() assert all(slider.removed is True for slider in stale_sliders) - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [ + assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [ -0.738, -0.2826151825863572, ] def test_gui_parses_numpy_transform_control_arrays() -> None: - pose = pose_from_transform_values( - np.array([1.0, 2.0, 3.0]), - np.array([0.5, 0.1, 0.2, 0.3]), + gui = ViserPanelGui(FakeGuiServer(), make_adapter_with_robot(), ViserVisualizationConfig()) + + pose = gui._pose_from_transform_target( + SimpleNamespace( + position=np.array([1.0, 2.0, 3.0]), + wxyz=np.array([0.5, 0.1, 0.2, 0.3]), + ) ) + assert pose is not None assert list(pose.position) == [1.0, 2.0, 3.0] assert list(pose.orientation) == [0.1, 0.2, 0.3, 0.5] @@ -1518,6 +1212,8 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( config = make_robot_config(joint_names=["j1"], home_joints=[0.5]) module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, + _planned_paths={}, + _planned_trajectories={}, execute=lambda robot_name=None: False, ) world_monitor = SimpleNamespace( @@ -1526,13 +1222,16 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - module_context = (world_monitor, module) - gui = make_panel(FakeGuiServer(), module_context) + adapter = InProcessViserAdapter( + world_monitor=world_monitor, + manipulation_module=module, + ) + gui = make_panel(FakeGuiServer(), adapter) gui.refresh() assert gui.state.selected_robot == "arm" - assert list(gui._joint_sliders) == ["arm/j1"] + assert list(gui._joint_sliders) == ["j1"] gui._apply_preset("Home") - assert gui._joint_sliders["arm/j1"].value == 0.5 + assert gui._joint_sliders["j1"].value == 0.5 gui._submit_execute() assert "Panel execution disabled" in gui.state.error @@ -1544,14 +1243,16 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) target_pose = SimpleNamespace(position=SimpleNamespace(x=0.2, y=0.3, z=0.4)) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} + ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: target_pose, ) - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) target_updates = [] target_pose_updates = [] scene = SimpleNamespace( @@ -1561,56 +1262,45 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) requests = [] gui._worker.stop() gui._worker = SimpleNamespace( submit=lambda request: requests.append(request), stop=lambda: None ) - gui._joint_sliders["arm/j1"].value = 0.25 - gui._joint_sliders["arm/j2"].value = 0.75 + gui._joint_sliders["j1"].value = 0.25 + gui._joint_sliders["j2"].value = 0.75 gui._submit_joint_target_evaluation() assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) - assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, target_pose) + assert target_pose_updates[-1] == ("robot-1", target_pose) assert requests[-1].source == "joints" - stale_request = TargetEvaluationRequest( - sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) - ) - fresh_request = TargetEvaluationRequest( - sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) - ) + stale_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") + fresh_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") gui.state.latest_sequence_id = 2 gui._apply_target_evaluation_result( stale_request, { "success": True, "collision_free": True, - "target_joints": joints_from_values(["arm/j1", "arm/j2"], [9.0, 9.0]), + "joint_state": adapter.joints_from_values(["j1", "j2"], [9.0, 9.0]), }, ) - assert gui.state.target_joints is not None - assert list(gui.state.target_joints.position) == [0.25, 0.75] + assert gui.state.joint_target == [0.25, 0.75] - joint_bar_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) gui._apply_target_evaluation_result( fresh_request, { "success": True, "collision_free": True, - "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), - "group_poses": {DEFAULT_GROUP_ID: joint_bar_pose}, + "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), }, ) assert gui.state.target_status == TargetStatus.FEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.FEASIBLE - assert gui.state.target_joints is not None - assert list(gui.state.target_joints.position) == [1.0, 2.0] - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.75] + assert gui.state.joint_target == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.75] assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) - assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, joint_bar_pose) def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( @@ -1618,35 +1308,30 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} + ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) target_joint_updates = [] target_pose_updates = [] scene = SimpleNamespace( has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: object(), + ensure_target_controls=lambda *args: None, set_target_joints=lambda *args: target_joint_updates.append(args) or True, set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - gui._handles[f"ee_control:{DEFAULT_GROUP_ID}"] = object() - dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) - solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) - gui.state.cartesian_target = dragged_pose - gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose - target_pose_updates.clear() - request = TargetEvaluationRequest( - sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + gui.state.cartesian_target = Pose( + {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} ) + request = TargetEvaluationRequest(sequence_id=1, source="cartesian", robot_name="arm") gui.state.latest_sequence_id = 1 gui._apply_target_evaluation_result( @@ -1654,128 +1339,14 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( { "success": True, "collision_free": True, - "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), - "group_poses": {DEFAULT_GROUP_ID: solved_pose}, + "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), }, ) assert gui.state.target_status == TargetStatus.FEASIBLE - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) assert target_pose_updates == [] - assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose - assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose - - -def test_gui_cartesian_collision_still_updates_target_ghost_red( - make_panel: Callable[..., ViserPanelGui], -) -> None: - current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) - config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: current, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=lambda robot_id, joint_state: False, - get_ee_pose=lambda robot_id, joint_state=None: None, - ) - module_context = (world_monitor, module) - target_joint_updates = [] - target_pose_updates = [] - visual_states = [] - scene = SimpleNamespace( - has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: object(), - set_target_joints=lambda *args: target_joint_updates.append(args) or True, - set_target_pose=lambda *args: target_pose_updates.append(args), - set_target_visual_state=lambda *args: visual_states.append(args), - ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) - solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) - gui.state.cartesian_target = dragged_pose - gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose - target_joint_updates.clear() - target_pose_updates.clear() - visual_states.clear() - request = TargetEvaluationRequest( - sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) - ) - gui.state.latest_sequence_id = 1 - - gui._apply_target_evaluation_result( - request, - { - "success": False, - "status": "COLLISION", - "message": "Target is in collision", - "collision_free": False, - "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), - "group_poses": {DEFAULT_GROUP_ID: solved_pose}, - }, - ) - - assert gui.state.target_status == TargetStatus.INFEASIBLE - assert gui.state.feasibility.status == FeasibilityStatus.COLLISION - assert gui.state.target_joints is not None - assert list(gui.state.target_joints.position) == [1.0, 2.0] - assert gui.state.last_valid_target_joints is None - assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] - assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) - assert (DEFAULT_GROUP_ID, False) in visual_states - assert ("robot-1", False) in visual_states - assert target_pose_updates == [] - assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose - assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose - - -def test_gui_can_disable_collision_check_for_cartesian_target_evaluation( - make_panel: Callable[..., ViserPanelGui], -) -> None: - current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) - config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.0, 0.0]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: current, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=lambda robot_id, joint_state: False, - get_ee_pose=lambda robot_id, joint_state=None: Pose( - {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} - ), - ) - module_context = (world_monitor, module) - scene = SimpleNamespace( - has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: None, - set_target_joints=lambda *args: True, - set_target_pose=lambda *args: None, - set_target_visual_state=lambda *args: None, - ) - gui = make_panel( - FakeGuiServer(), - module_context, - ViserVisualizationConfig(panel_enabled=True), - scene, - ) - request = TargetEvaluationRequest( - sequence_id=1, - source="cartesian", - group_ids=(DEFAULT_GROUP_ID,), - pose_targets={ - DEFAULT_GROUP_ID: Pose( - {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} - ) - }, - check_collision=False, - ) - - result = gui._handle_target_evaluation_request(request) - - assert result["success"] is True - assert result["collision_free"] is True - assert result["message"] == "Target collision check skipped" def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( @@ -1783,7 +1354,9 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( ) -> None: current = FakeJointState(["j1"], position=[0.0]) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} + ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1793,7 +1366,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( ), ) module._world_monitor = world_monitor - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) visual_states = [] scene = SimpleNamespace( has_reference_grid=lambda: False, @@ -1802,14 +1375,10 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( set_target_pose=lambda *args: None, set_target_visual_state=lambda *args: visual_states.append(args), ) - gui = make_panel( - FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene - ) - request = TargetEvaluationRequest(sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,)) + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") gui.state.latest_sequence_id = 1 - result = gui._evaluate_joint_target_set( - {DEFAULT_GROUP_ID: FakeJointState(["arm/j1"], position=[1.0])} - ) + result = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[1.0]), "arm") gui._apply_target_evaluation_result(request, result) @@ -1817,8 +1386,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( assert gui.state.target_status == TargetStatus.INFEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.COLLISION assert gui.state.error == "Target is in collision" - assert (DEFAULT_GROUP_ID, False) in visual_states - assert ("robot-1", False) in visual_states + assert visual_states[-1] == ("robot-1", False) def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( @@ -1832,6 +1400,8 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( _robots={ "arm": ("robot-1", make_robot_config(joint_names=["j1"], home_joints=[1.0]), None) }, + _planned_paths={"arm": planned}, + _planned_trajectories={}, _state=NamedState(name="IDLE"), execute=lambda robot_name=None: executed.append(robot_name) or True, clear_planned_path=lambda: cleared.append(True) or True, @@ -1844,10 +1414,10 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( position=SimpleNamespace(x=0.0, y=0.0, z=0.0) ), ) - module_context = (world_monitor, module) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) gui = make_panel( FakeGuiServer(), - module_context, + adapter, ViserVisualizationConfig( panel_enabled=True, allow_plan_execute=True, current_match_tolerance=0.05 ), @@ -1863,20 +1433,19 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.target_status = TargetStatus.FEASIBLE gui.state.plan_state = PanelPlanState( status=PlanStatus.FRESH, - group_ids=(DEFAULT_GROUP_ID,), - start_joints_snapshot={DEFAULT_GROUP_ID: [1.2]}, + robot="arm", + start_joints_snapshot=[1.2], planned_path=planned, ) - gui.state.target_joints = FakeJointState(["arm/j1"], position=[2.0]) gui._submit_execute() assert executed == [] assert "Cannot execute" in gui.state.error gui.state.action_status = ActionStatus.IDLE gui.state.error = "" - gui.state.plan_state.start_joints_snapshot = {DEFAULT_GROUP_ID: [1.0]} + gui.state.plan_state.start_joints_snapshot = [1.0] gui._submit_execute() - assert executed == [None] + assert executed == ["arm"] gui._submit_clear() assert cleared == [True] @@ -1887,8 +1456,8 @@ def test_gui_plan_target_failure_recovers_action_state( make_panel: Callable[..., ViserPanelGui], monkeypatch: pytest.MonkeyPatch, ) -> None: - module_context = make_module_with_robot() - gui = make_panel(FakeGuiServer(), module_context) + adapter = make_adapter_with_robot() + gui = make_panel(FakeGuiServer(), adapter) gui._operation_worker.stop() monkeypatch.setattr( gui, @@ -1897,8 +1466,7 @@ def test_gui_plan_target_failure_recovers_action_state( submit=lambda operation, **_kwargs: operation(), stop=lambda timeout=2.0: None ), ) - gui.state.selected_group_ids = ("missing",) - gui.state.target_joints = JointState({"name": ["missing/j1"], "position": [1.0]}) + gui.state.selected_robot = "missing" gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -1906,7 +1474,7 @@ def test_gui_plan_target_failure_recovers_action_state( assert gui.state.action_status == ActionStatus.IDLE assert gui.state.plan_state.status == PlanStatus.FAILED - assert gui.state.error == "Unknown planning group: missing" + assert gui.state.error == "No robot config" assert gui.state.last_result == "plan_to_joints=False" @@ -1915,8 +1483,8 @@ def test_gui_resets_fault_before_replanning( monkeypatch: pytest.MonkeyPatch, ) -> None: calls = [] - module_context = make_module_with_robot() - gui = make_panel(FakeGuiServer(), module_context) + adapter = make_adapter_with_robot() + gui = make_panel(FakeGuiServer(), adapter) gui._operation_worker.stop() monkeypatch.setattr( gui, @@ -1930,12 +1498,12 @@ def reset() -> bool: calls.append("reset") return True - def plan_target_set(_joint_targets: dict[str, JointState]) -> bool: + def plan_to_joints(_joints: JointState, _robot_name: str | None = None) -> bool: calls.append("plan") return True - monkeypatch.setattr(module_context[1], "reset", reset) - monkeypatch.setattr(module_context[1], "plan_to_joint_targets", plan_target_set) + monkeypatch.setattr(adapter, "reset", reset) + monkeypatch.setattr(adapter, "plan_to_joints", plan_to_joints) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "FAULT" @@ -1994,12 +1562,8 @@ def operation() -> None: def test_target_evaluation_worker_coalesces_pending_requests() -> None: worker = TargetEvaluationWorker(lambda request: {}, lambda request, result: None) - old_request = TargetEvaluationRequest( - sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) - ) - new_request = TargetEvaluationRequest( - sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) - ) + old_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") + new_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") worker.submit(old_request) worker.submit(new_request) diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 36977ce313..19f1f2da60 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -22,13 +22,12 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.enums import PlanningStatus -from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningSceneInfo +from dimos.manipulation.planning.spec.models import PlanningSceneInfo from dimos.manipulation.visualization.viser import ( runtime as runtime_module, visualizer as visualizer_module, ) -from dimos.manipulation.visualization.viser.animation import GroupPreviewAnimation +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import ViserRuntime from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer @@ -63,7 +62,7 @@ def fake_robot_config(name: str) -> RobotModelConfig: name=name, model_path=Path(f"{name}.urdf"), base_pose=PoseStamped(), - joint_names=["joint1"], + joint_names=[], end_effector_link="ee_link", ) @@ -122,12 +121,10 @@ class FakeGui: def __init__( self, server: FakeServer, - world_monitor: object, - manipulation_module: object, + adapter: InProcessViserAdapter, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: - del world_monitor, manipulation_module, config, scene calls.append(("create", "gui")) def start(self) -> None: @@ -202,12 +199,10 @@ class FakeGui: def __init__( self, server: FakeServer, - world_monitor: object, - manipulation_module: object, + adapter: InProcessViserAdapter, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: - del world_monitor, manipulation_module, config, scene pass def start(self) -> None: @@ -310,12 +305,10 @@ class FailingGui: def __init__( self, server: FakeServer, - world_monitor: object, - manipulation_module: object, + adapter: InProcessViserAdapter, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: - del world_monitor, manipulation_module, config, scene pass def start(self) -> None: @@ -414,30 +407,18 @@ def show_preview(self, robot_id: str) -> None: def hide_preview(self, robot_id: str) -> None: calls.append(("hide", robot_id)) - def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: - assert preview.group_ids == ("arm/manipulator",) - assert len(preview.tracks) == 1 - assert preview.tracks[0].path == (current,) + def animate_path(self, robot_id: str, path: list[JointState], duration: float) -> None: + assert path == [current] assert duration == 1.5 - calls.append(("animate", preview.tracks[0].robot_id)) + calls.append(("animate", robot_id)) def close(self) -> None: calls.append(("scene", "close")) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda _robot_id: current, - planning_groups=SimpleNamespace( - select=lambda _group_ids: SimpleNamespace( - groups=(SimpleNamespace(id="arm/manipulator", robot_name="arm"),), - robot_names=("arm",), - ) - ), - ) - robot_config = fake_robot_config("arm") + world_monitor = SimpleNamespace(get_current_joint_state=lambda _robot_id: current) manipulation_module = SimpleNamespace( - robot_items=lambda: [("arm", "robot-1", robot_config)], + robot_items=lambda: [("arm", "robot-1", fake_robot_config("arm"))], robot_id_for_name=lambda robot_name: "robot-1" if robot_name == "arm" else None, - get_robot_config=lambda robot_name: robot_config if robot_name == "arm" else None, ) monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) @@ -449,14 +430,9 @@ def close(self) -> None: ) visualizer.publish_visualization() - visualizer.show_preview(("arm/manipulator",)) - visualizer.hide_preview(("arm/manipulator",)) - plan = GeneratedPlan( - group_ids=("arm/manipulator",), - path=[JointState(name=["arm/joint1"], position=[0.5])], - status=PlanningStatus.SUCCESS, - ) - visualizer.animate_plan(plan, duration=1.5) + visualizer.show_preview("robot-1") + visualizer.hide_preview("robot-1") + visualizer.animate_path("robot-1", [current], duration=1.5) visualizer.close() visualizer.publish_visualization() @@ -470,86 +446,3 @@ def close(self) -> None: ("scene", "close"), ("runtime", "close"), ] - - -def test_visualizer_animates_multi_robot_plan_as_one_group_preview( - monkeypatch: pytest.MonkeyPatch, -) -> None: - previews: list[GroupPreviewAnimation] = [] - - class FakeRuntime: - url = "http://localhost:8095" - - def __init__(self, config: ViserVisualizationConfig) -> None: - self.config = config - - def start(self) -> FakeServer: - return FakeServer() - - def close(self) -> None: - pass - - class FakeScene: - def __init__( - self, - server: FakeServer, - viser_urdf: type[FakeViserUrdf], - *, - preview_fps: float, - ) -> None: - pass - - def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: - assert duration == 2.0 - previews.append(preview) - - def close(self) -> None: - pass - - groups = ( - SimpleNamespace(id="left/arm", robot_name="left"), - SimpleNamespace(id="right/arm", robot_name="right"), - ) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: JointState( - {"name": ["joint1"], "position": [0.0 if robot_id == "left-id" else 10.0]} - ), - planning_groups=SimpleNamespace( - select=lambda _group_ids: SimpleNamespace(groups=groups, robot_names=("left", "right")) - ), - ) - configs = {"left": fake_robot_config("left"), "right": fake_robot_config("right")} - manipulation_module = SimpleNamespace( - robot_id_for_name=lambda robot_name: f"{robot_name}-id" if robot_name in configs else None, - get_robot_config=lambda robot_name: configs.get(robot_name), - ) - monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) - monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) - monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) - visualizer = ViserManipulationVisualizer( - world_monitor=world_monitor, - manipulation_module=manipulation_module, - config=ViserVisualizationConfig(panel_enabled=False), - ) - - visualizer.animate_plan( - GeneratedPlan( - group_ids=("left/arm", "right/arm"), - path=[ - JointState(name=["left/joint1", "right/joint1"], position=[0.0, 10.0]), - JointState(name=["left/joint1", "right/joint1"], position=[1.0, 11.0]), - ], - status=PlanningStatus.SUCCESS, - ), - duration=2.0, - ) - - assert len(previews) == 1 - preview = previews[0] - assert preview.group_ids == ("left/arm", "right/arm") - assert [(track.robot_id, track.group_ids) for track in preview.tracks] == [ - ("left-id", ("left/arm",)), - ("right-id", ("right/arm",)), - ] - assert [tuple(point.position) for point in preview.tracks[0].path] == [(0.0,), (1.0,)] - assert [tuple(point.position) for point in preview.tracks[1].path] == [(10.0,), (11.0,)] diff --git a/dimos/manipulation/visualization/viser/theme.py b/dimos/manipulation/visualization/viser/theme.py index d38767d1da..339be04492 100644 --- a/dimos/manipulation/visualization/viser/theme.py +++ b/dimos/manipulation/visualization/viser/theme.py @@ -36,7 +36,7 @@ DIMOS_THEME_TITLE = "DimOS Manipulation" DIMOS_THEME_URL = "https://github.com/dimensionalOS/dimos" -DIMOS_BRAND_COLOR = (0, 153, 255) +DIMOS_BRAND_COLOR = (22, 130, 163) DIMOS_LOGO_PATH = Path(__file__).with_name("assets") / "dimensional-logo.svg" logger = setup_logger() @@ -86,8 +86,8 @@ def _configure_theme(server: ViserServer, titlebar_content: TitlebarConfig | Non try: server.gui.configure_theme( titlebar_content=titlebar_content, - control_layout="fixed", - control_width="large", + control_layout="collapsible", + control_width="medium", dark_mode=True, show_logo=False, show_share_button=False, diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 62c7c2cf45..2a44008118 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -19,10 +19,7 @@ from typing import TYPE_CHECKING from dimos.manipulation.planning.groups.identifiers import make_global_joint_name -from dimos.manipulation.visualization.viser.animation import ( - GroupPreviewAnimation, - PreviewTrack, -) +from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.runtime import ( @@ -74,6 +71,7 @@ def __init__( self.config = config or ViserVisualizationConfig() self._runtime: ViserRuntime | None = None self._server: ViserServer | None = None + self._adapter: InProcessViserAdapter | None = None self._scene: ViserManipulationScene | None = None self._gui: ViserPanelGui | None = None self._closed = False @@ -92,11 +90,14 @@ def _ensure_started(self) -> None: ViserUrdf, preview_fps=self.config.preview_fps, ) + adapter = InProcessViserAdapter( + world_monitor=self._world_monitor, + manipulation_module=self._manipulation_module, + ) gui = ( ViserPanelGui( server, - self._world_monitor, - self._manipulation_module, + adapter, self.config, scene, ) @@ -116,12 +117,14 @@ def _ensure_started(self) -> None: runtime.close() self._runtime = None self._server = None + self._adapter = None self._scene = None self._gui = None self._closed = True raise self._runtime = runtime self._server = server + self._adapter = adapter self._scene = scene self._gui = gui self._closed = False @@ -151,17 +154,10 @@ def publish_visualization(self, ctx: None = None) -> None: if self._closed: return self._ensure_started() - if self._scene is None: + if self._adapter is None or self._scene is None: return - for robot_name, robot_id, _config in self._manipulation_module.robot_items(): - get_current_joint_state = getattr( - self._manipulation_module, "get_current_joint_state", None - ) - current = ( - get_current_joint_state(robot_name) - if callable(get_current_joint_state) - else self._world_monitor.get_current_joint_state(robot_id) - ) + for robot_name, robot_id, _config in self._adapter.robot_items(): + current = self._adapter.get_current_joint_state(robot_name) self._scene.update_current_robot(str(robot_id), current) if self._gui is not None: self._gui.refresh() @@ -182,57 +178,50 @@ def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: for robot_id in self._robot_ids_for_groups(group_ids): self._scene.hide_preview(str(robot_id)) + def animate_path( + self, + robot_id: str, + path: list[JointState], + duration: float = 3.0, + ) -> None: + """Compatibility wrapper for legacy robot-scoped Viser callers.""" + if self._closed: + return + self._ensure_started() + if self._scene is not None: + self._scene.animate_path(str(robot_id), path, duration) + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: if self._closed: return self._ensure_started() if self._scene is None: return - preview = self._build_group_preview_animation(plan) - if preview is not None: - self._scene.animate_preview(preview, duration) - - def _build_group_preview_animation(self, plan: GeneratedPlan) -> GroupPreviewAnimation | None: - selection = self._world_monitor.planning_groups.select(plan.group_ids) - tracks: list[PreviewTrack] = [] - for robot_name in selection.robot_names: + for robot_name in self._robot_names_for_groups(plan.group_ids): robot_id = self._manipulation_module.robot_id_for_name(robot_name) config = self._manipulation_module.get_robot_config(robot_name) - get_current_joint_state = getattr( - self._manipulation_module, "get_current_joint_state", None - ) - current = ( - get_current_joint_state(robot_name) - if callable(get_current_joint_state) - else self._world_monitor.get_current_joint_state(robot_id) - if robot_id is not None - else None - ) + current = self._manipulation_module.get_current_joint_state(robot_name) if robot_id is None or config is None or current is None: logger.warning( "Cannot build group preview for robot '%s': missing id, config, or state", robot_name, ) - return None + return path = self._robot_path_for_plan(robot_name, config, current, plan) if not path: logger.warning("Cannot project generated plan for robot '%s'", robot_name) - return None - tracks.append( - PreviewTrack( - robot_id=str(robot_id), - group_ids=tuple( - group.id for group in selection.groups if group.robot_name == robot_name - ), - joint_names=tuple(config.joint_names), - path=tuple(path), - ) - ) - if not tracks: - return None - return GroupPreviewAnimation(group_ids=plan.group_ids, tracks=tuple(tracks)) + return + self._scene.animate_path(str(robot_id), path, duration) + + def _robot_names_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: + selection = self._world_monitor.planning_groups.select(group_ids) + return list(selection.robot_names) def _robot_ids_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: + if isinstance(group_ids, str): + return [group_ids] + if not hasattr(self._world_monitor, "planning_groups"): + return [str(group_id) for group_id in group_ids] selection = self._world_monitor.planning_groups.select(group_ids) robot_ids: list[str] = [] for robot_name in selection.robot_names: @@ -310,6 +299,7 @@ def close(self) -> None: errors.append(e) self._runtime = None self._server = None + self._adapter = None self._scene = None self._gui = None if errors: From bfbac26d787c1205d115dac595c44f0fa6dc7328 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 22 Jun 2026 15:09:00 -0700 Subject: [PATCH 8/8] feat: move planning group viser panel to stacked pr --- .../visualization/viser/adapter.py | 241 ----- .../visualization/viser/animation.py | 33 +- dimos/manipulation/visualization/viser/gui.py | 944 ++++++++++++----- .../visualization/viser/panel_backend.py | 268 +++++ .../manipulation/visualization/viser/scene.py | 158 ++- .../manipulation/visualization/viser/state.py | 53 +- .../visualization/viser/test_gui_status.py | 44 +- .../viser/test_operation_worker.py | 35 +- .../visualization/viser/test_state.py | 3 + .../viser/test_viser_visualization.py | 948 +++++++++++++----- .../viser/test_visualizer_lifecycle.py | 135 ++- .../manipulation/visualization/viser/theme.py | 6 +- .../visualization/viser/visualizer.py | 88 +- 13 files changed, 2087 insertions(+), 869 deletions(-) delete mode 100644 dimos/manipulation/visualization/viser/adapter.py create mode 100644 dimos/manipulation/visualization/viser/panel_backend.py diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py deleted file mode 100644 index 48b84c6f55..0000000000 --- a/dimos/manipulation/visualization/viser/adapter.py +++ /dev/null @@ -1,241 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Sequence -from typing import TYPE_CHECKING, cast - -from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation -from dimos.msgs.sensor_msgs.JointState import JointState - -if TYPE_CHECKING: - from dimos.manipulation.manipulation_module import ManipulationModule - from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor - from dimos.manipulation.planning.spec.config import RobotModelConfig - from dimos.manipulation.planning.spec.models import JointPath, RobotName, WorldRobotID - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - -def copy_joint_state(joint_state: JointState | None) -> JointState | None: - """Make a local copy of a JointState-like message for rendering.""" - return None if joint_state is None else JointState(joint_state) - - -class InProcessViserAdapter: - """Small in-process boundary between Viser callbacks and manipulation internals.""" - - def __init__( - self, - *, - world_monitor: WorldMonitor, - manipulation_module: ManipulationModule, - ) -> None: - self._world_monitor = world_monitor - self._module = manipulation_module - - def list_robots(self) -> list[RobotName]: - return list(self._module.list_robots()) - - def robot_items(self) -> list[tuple[RobotName, WorldRobotID, RobotModelConfig]]: - return self._module.robot_items() - - def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: - return self._module.robot_id_for_name(robot_name) - - def robot_name_for_id(self, robot_id: WorldRobotID) -> RobotName | None: - return self._module.robot_name_for_id(robot_id) - - def get_robot_config(self, robot_name: RobotName) -> RobotModelConfig | None: - return self._module.get_robot_config(robot_name) - - def get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: - info = self._module.get_robot_info(robot_name) - if info is None: - return None - return info - - def get_init_joints(self, robot_name: RobotName) -> JointState | None: - return copy_joint_state(self._module.get_init_joints(robot_name)) - - def get_current_joint_state(self, robot_name: RobotName) -> JointState | None: - robot_id = self.robot_id_for_name(robot_name) - if robot_id is None: - return None - return copy_joint_state(self._world_monitor.get_current_joint_state(robot_id)) - - def is_state_stale(self, robot_name: RobotName, max_age: float = 1.0) -> bool: - robot_id = self.robot_id_for_name(robot_name) - return True if robot_id is None else self._world_monitor.is_state_stale(robot_id, max_age) - - def get_ee_pose( - self, robot_name: RobotName, joint_state: JointState | None = None - ) -> PoseStamped | None: - robot_id = self.robot_id_for_name(robot_name) - if robot_id is None: - return None - return self._world_monitor.get_ee_pose(robot_id, copy_joint_state(joint_state)) - - def evaluate_joint_target(self, joints: JointState, robot_name: RobotName) -> TargetEvaluation: - """Evaluate a legacy robot-scoped joint target through group-scoped APIs.""" - legacy_evaluate = getattr(self._module, "evaluate_joint_target", None) - if callable(legacy_evaluate): - result = cast("TargetEvaluation", legacy_evaluate(JointState(joints), robot_name)) - joint_state = result.get("joint_state") - result["joint_state"] = copy_joint_state( - joint_state if isinstance(joint_state, JointState) else None - ) - return result - default_group_id = getattr(self._module, "_default_group_id_for_robot", None) - joint_target_to_global = getattr(self._module, "_joint_target_to_global_names", None) - if not callable(default_group_id) or not callable(joint_target_to_global): - robot_id = self.robot_id_for_name(robot_name) - valid = ( - False - if robot_id is None - else bool(self._world_monitor.is_state_valid(robot_id, JointState(joints))) - ) - return { - "success": valid, - "status": "FEASIBLE" if valid else "COLLISION", - "message": "Target is valid" if valid else "Target is in collision", - "collision_free": valid, - "joint_state": JointState(joints), - } - group_id = self._module._default_group_id_for_robot(robot_name) - if group_id is None: - return {"success": False, "status": "INVALID", "message": "No default group"} - target = self._module._joint_target_to_global_names(group_id, JointState(joints)) - if target is None: - return {"success": False, "status": "INVALID", "message": "Invalid joint target"} - collision = self._module.check_collision(target) - collision_free = collision.collision_free is True - return { - "success": collision_free, - "status": collision.status, - "message": collision.message, - "collision_free": collision_free, - "joint_state": copy_joint_state(target), - } - - def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: - """Evaluate a legacy robot-scoped pose target through group-scoped IK.""" - ik = self._module.inverse_kinematics_single(pose, robot_name=robot_name) - if not ik.is_success() or ik.joint_state is None: - return { - "success": False, - "status": ik.status.name, - "message": ik.message, - "collision_free": False, - "joint_state": None, - "position_error": ik.position_error, - "orientation_error": ik.orientation_error, - } - collision = self._module.check_collision(ik.joint_state) - collision_free = collision.collision_free is True - return { - "success": collision_free, - "status": collision.status if not collision_free else IKStatus.SUCCESS.name, - "message": collision.message, - "collision_free": collision_free, - "joint_state": copy_joint_state(ik.joint_state), - "position_error": ik.position_error, - "orientation_error": ik.orientation_error, - } - - def get_planned_path(self, robot_name: RobotName) -> JointPath | None: - legacy_get_planned_path = getattr(self._module, "get_planned_path", None) - if callable(legacy_get_planned_path): - legacy_path = legacy_get_planned_path(robot_name) - if legacy_path is None: - return None - if not isinstance(legacy_path, list): - return None - copied = [copy_joint_state(point) for point in legacy_path] - return [point for point in copied if point is not None] - planned_paths = getattr(self._module, "_planned_paths", None) - if isinstance(planned_paths, dict): - path_obj = planned_paths.get(robot_name) - if isinstance(path_obj, list): - copied = [copy_joint_state(point) for point in path_obj] - return [point for point in copied if point is not None] - plan = getattr(self._module, "_last_plan", None) - config = self.get_robot_config(robot_name) - current = self.get_current_joint_state(robot_name) - if plan is None or config is None or current is None: - return None - path: JointPath = [] - current_by_name = dict(zip(current.name, current.position, strict=False)) - for waypoint in plan.path: - selected = dict(zip(waypoint.name, waypoint.position, strict=False)) - positions: list[float] = [] - for local_name in config.joint_names: - global_name = f"{robot_name}/{local_name}" - if global_name in selected: - positions.append(float(selected[global_name])) - elif local_name in current_by_name: - positions.append(float(current_by_name[local_name])) - else: - return None - path.append(JointState(name=list(config.joint_names), position=positions)) - return path - - def get_planned_trajectory_duration(self, robot_name: RobotName) -> float | None: - path = self.get_planned_path(robot_name) - return None if path is None else float(max(len(path) - 1, 0)) - - def get_module_state(self) -> str: - return str(self._module.get_state()) - - def get_error(self) -> str: - return self._module.get_error() - - def reset(self) -> bool: - return self._module.reset().is_success() - - def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: - return self._module.plan_to_pose(pose, robot_name) - - def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: - return self._module.plan_to_joints(joints, robot_name) - - def preview_path(self, robot_name: RobotName | None = None) -> object: - preview_path = getattr(self._module, "preview_path", None) - if callable(preview_path): - return preview_path(robot_name=robot_name) - return self._module.preview_plan(robot_name=robot_name) - - def execute(self, robot_name: RobotName | None = None) -> bool: - execute_plan = getattr(self._module, "execute_plan", None) - if callable(execute_plan): - return bool(execute_plan()) - execute = getattr(self._module, "execute", None) - return bool(execute(robot_name)) if callable(execute) else False - - def cancel(self) -> bool: - return self._module.cancel() - - def clear_planned_path(self) -> bool: - return self._module.clear_planned_path() - - @staticmethod - def joints_from_values(joint_names: Sequence[str], values: Sequence[float]) -> JointState: - return JointState( - { - "name": list(joint_names), - "position": [float(value) for value in values], - } - ) diff --git a/dimos/manipulation/visualization/viser/animation.py b/dimos/manipulation/visualization/viser/animation.py index f5a574d82c..03b4e3321e 100644 --- a/dimos/manipulation/visualization/viser/animation.py +++ b/dimos/manipulation/visualization/viser/animation.py @@ -15,11 +15,31 @@ from __future__ import annotations from collections.abc import Callable, Sequence +from dataclasses import dataclass import time +from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.msgs.sensor_msgs.JointState import JointState +@dataclass(frozen=True) +class PreviewTrack: + """One render track owned by one or more planning groups.""" + + robot_id: str + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[str, ...] + path: tuple[JointState, ...] + + +@dataclass(frozen=True) +class GroupPreviewAnimation: + """Group-native preview transaction for a generated plan.""" + + group_ids: tuple[PlanningGroupID, ...] + tracks: tuple[PreviewTrack, ...] + + def interpolate_joint_path( path: Sequence[JointState], duration: float, fps: float ) -> list[list[float]]: @@ -57,10 +77,10 @@ def sampled_joint_path_frames( ) -> list[list[float]]: """Return animation frames while preserving already sampled trajectories. - ManipulationModule.preview_path() owns trajectory-aware interpolation because it has access - to JointTrajectory waypoint timing. If a path arrives already sampled near the target display - rate, Viser should play those samples directly instead of re-interpolating by waypoint index. - Sparse direct VisualizationSpec callers still get local interpolation as a fallback. + ViserManipulationVisualizer.animate_plan() projects GeneratedPlan waypoints into robot-local + preview paths. If a path arrives already sampled near the target display rate, Viser should + play those samples directly instead of re-interpolating by waypoint index. Sparse direct scene + callers still get local interpolation as a fallback. """ waypoints = [list(waypoint.position) for waypoint in path if waypoint.position] if not waypoints: @@ -92,7 +112,8 @@ def animate(self, path: Sequence[JointState], duration: float, fps: float) -> bo if not frames: return False step_delay = duration / max(len(frames) - 1, 1) if duration > 0.0 else 0.0 - for joints in frames: + for index, joints in enumerate(frames): self._set_joints(joints) - self._sleep(step_delay) + if index < len(frames) - 1: + self._sleep(step_delay) return True diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 0434228583..175d900238 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -14,11 +14,29 @@ from __future__ import annotations -from typing import TypeAlias - -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from collections.abc import Callable, Mapping +from typing import TYPE_CHECKING, TypeAlias, cast + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName +from dimos.manipulation.visualization.types import ( + RobotInfo, + TargetEvaluation, + TargetSetEvaluation, +) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig +from dimos.manipulation.visualization.viser.panel_backend import ( + copy_joint_state, + evaluate_joint_target_set, + evaluate_pose_target_set, + feasibility_status, + get_current_joint_state, + get_ee_pose, + is_state_stale, + joint_values_by_name, + pose_from_transform_values, + update_target_visual_state, +) from dimos.manipulation.visualization.viser.runtime import VISER_INSTALL_HINT from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( @@ -38,6 +56,10 @@ from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger +if TYPE_CHECKING: + from dimos.manipulation.manipulation_module import ManipulationModule + from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor + logger = setup_logger() try: @@ -68,6 +90,23 @@ # Fallback joint-slider range (radians) when a robot config omits joint limits. DEFAULT_JOINT_LIMITS = (-3.14, 3.14) +PRIMARY_ACTION_COLOR = (0, 102, 179) +ACTIVE_GROUP_COLOR = PRIMARY_ACTION_COLOR +INACTIVE_GROUP_COLOR = (52, 52, 52) + + +def group_display_name(group: PlanningGroup) -> str: + robot_name = group.robot_name + group_name = group.group_name + return robot_name if group_name == "manipulator" else f"{robot_name} {group_name}" + + +def group_selector_color( + selected: bool, + active_color: tuple[int, int, int], + inactive_color: tuple[int, int, int], +) -> tuple[int, int, int]: + return active_color if selected else inactive_color class ViserPanelGui: @@ -76,18 +115,21 @@ class ViserPanelGui: def __init__( self, server: ViserServer, - adapter: InProcessViserAdapter, + world_monitor: WorldMonitor, + manipulation_module: ManipulationModule, config: ViserVisualizationConfig, scene: ViserManipulationScene | None = None, ) -> None: self.server = server - self.adapter = adapter + self.world_monitor = world_monitor + self.manipulation_module = manipulation_module self.config = config self.scene = scene self.state = PanelState(runtime=PanelRuntime.STARTING) self._closed = False self._operation_sequence_id = 0 self._suppress_target_callbacks = False + self._default_group_initialized = False self._handles: dict[str, PanelHandle] = {} self._joint_sliders: dict[str, GuiSliderHandle[float]] = {} self._worker = TargetEvaluationWorker( @@ -127,21 +169,93 @@ def close(self) -> None: def refresh(self) -> None: if self._closed: return - robots = self.adapter.list_robots() + robots = self._list_robots() + groups = self._list_planning_groups() self.state.backend_status = ( BackendConnectionStatus.READY if robots else BackendConnectionStatus.WAITING_FOR_ROBOT ) - if self.state.selected_robot is None and robots: - self.state.selected_robot = robots[0] + if not self.state.selected_group_ids and groups and not self._default_group_initialized: + first_pose_group = next((group for group in groups if group.has_pose_target), groups[0]) + self.state.selected_group_ids = (first_pose_group.id,) self.state.target_status = TargetStatus.EMPTY + self._default_group_initialized = True + self._sync_group_selection_state() + self._initialize_selected_group_targets() self._build_joint_sliders() - self._sync_robot_dropdown(robots) + self._sync_group_selector(groups) self._refresh_selected_robot_state() self._ensure_scene_controls() + self._sync_target_ghost_visibility() self._sync_preset_dropdown() self._update_status_text() + self._update_target_summary() self._update_control_state() + def _list_robots(self) -> list[RobotName]: + return list(self.manipulation_module.list_robots()) + + def _list_planning_groups(self) -> list[PlanningGroup]: + return self.manipulation_module.list_planning_groups() + + def _get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: + return self.manipulation_module.get_robot_info(robot_name) + + def _get_init_joints(self, robot_name: RobotName) -> JointState | None: + return copy_joint_state(self.manipulation_module.get_init_joints(robot_name)) + + def _get_current_joint_state(self, robot_name: RobotName) -> JointState | None: + return get_current_joint_state( + self.world_monitor, + self.manipulation_module, + robot_name, + ) + + def _is_state_stale(self, robot_name: RobotName, max_age: float = 1.0) -> bool: + return is_state_stale( + self.world_monitor, + self.manipulation_module, + robot_name, + max_age, + ) + + def _get_ee_pose( + self, robot_name: RobotName, joint_state: JointState | None = None + ) -> Pose | None: + return get_ee_pose( + self.world_monitor, + self.manipulation_module, + self._list_planning_groups(), + robot_name, + joint_state, + ) + + def _get_module_state(self) -> str: + return str(self.manipulation_module.get_state()) + + def _reset(self) -> bool: + result = self.manipulation_module.reset() + return result if isinstance(result, bool) else result.is_success() + + def _evaluate_joint_target_set( + self, joint_targets: dict[PlanningGroupID, JointState] + ) -> TargetSetEvaluation: + return evaluate_joint_target_set(self.manipulation_module, joint_targets) + + def _evaluate_pose_target_set( + self, + pose_targets: dict[PlanningGroupID, Pose], + auxiliary_groups: tuple[PlanningGroupID, ...] = (), + seed: JointState | None = None, + check_collision: bool = True, + ) -> TargetSetEvaluation: + return evaluate_pose_target_set( + self.manipulation_module, + pose_targets, + auxiliary_groups=auxiliary_groups, + seed=seed, + check_collision=check_collision, + ) + def _build(self) -> None: gui = self.server.gui folder = gui.add_folder("Manipulation Panel", expand_by_default=True) @@ -150,26 +264,28 @@ def _build(self) -> None: self._build_panel_controls(gui) def _build_panel_controls(self, gui: GuiApi) -> None: - self._handles["status"] = gui.add_markdown("Starting manipulation panel...") - robots = self.adapter.list_robots() + self._handles["status"] = gui.add_markdown("### Status\n**State:** Ready") self._build_scene_controls(gui) - robot_dropdown = gui.add_dropdown( - "Robot", - options=robots or [""], - initial_value=robots[0] if robots else "", + self._handles["planning_groups_heading"] = gui.add_markdown( + "### Planning Groups\nActive MoveIt group for pose goal, planning, and joint edits." ) - robot_dropdown.on_update(lambda event: self._select_robot(event.target.value)) - self._handles["robot"] = robot_dropdown + self._sync_group_selector(self._list_planning_groups()) + self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( - "Target Preset", + "Preset", options=["Select preset...", "Current"], initial_value="Select preset...", ) preset_dropdown.on_update(lambda event: self._apply_preset(event.target.value)) self._handles["preset"] = preset_dropdown - plan_button = gui.add_button("Plan", disabled=True) + self._handles["target_summary"] = gui.add_markdown( + f"Feasibility: `{self.state.feasibility.status.value}`" + ) + self._handles["actions_heading"] = gui.add_markdown("### Actions") + plan_button = gui.add_button("Plan", disabled=True, color=PRIMARY_ACTION_COLOR) plan_button.on_click(lambda _: self._submit_plan()) self._handles["plan"] = plan_button + self._handles["plan_controls_heading"] = gui.add_markdown("**Plan controls**") preview_button = gui.add_button("Preview", disabled=True) preview_button.on_click(lambda _: self._submit_preview()) self._handles["preview"] = preview_button @@ -182,6 +298,8 @@ def _build_panel_controls(self, gui: GuiApi) -> None: clear_button = gui.add_button("Clear plan") clear_button.on_click(lambda _: self._submit_clear()) self._handles["clear"] = clear_button + joint_controls = gui.add_folder("Joint Control", expand_by_default=False) + self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() def _build_scene_controls(self, gui: GuiApi) -> None: @@ -206,68 +324,152 @@ def _refresh_selected_robot_state(self) -> None: self.state.robot_info = None self.state.current_joints = None self.state.current_ee_pose = None - self.state.manipulation_state = self.adapter.get_module_state() + self.state.manipulation_state = self._get_module_state() return - self.state.robot_info = self.adapter.get_robot_info(robot_name) - current = self.adapter.get_current_joint_state(robot_name) + self.state.robot_info = self._get_robot_info(robot_name) + current = self._get_current_joint_state(robot_name) self.state.current_joints = list(current.position) if current is not None else None - self.state.current_ee_pose = self.adapter.get_ee_pose(robot_name) - self.state.manipulation_state = self.adapter.get_module_state() - adapter_error = self.adapter.get_error() + self.state.current_ee_pose = self._get_ee_pose(robot_name) + self.state.manipulation_state = self._get_module_state() + adapter_error = self.manipulation_module.get_error() if adapter_error: self.state.error = adapter_error def _ensure_scene_controls(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - ee_control = self.scene.ensure_target_controls(str(robot_id), self._on_transform_update) - if ee_control is not None: - self._handles["ee_control"] = ee_control - if ( - self.state.target_status == TargetStatus.EMPTY - and self.state.current_ee_pose is not None - ): - self.state.cartesian_target = self.state.current_ee_pose - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), self.state.current_ee_pose) - finally: - self._suppress_target_callbacks = False + groups = self._group_info_by_id() + active_pose_groups = set(self._selected_pose_group_ids()) + for key in [key for key in self._handles if key.startswith("ee_control:")]: + group_id = key.split(":", 1)[1] + if group_id in active_pose_groups: + continue + handle = self._handles.pop(key) + remove_target_controls = getattr(self.scene, "remove_target_controls", None) + if callable(remove_target_controls): + remove_target_controls(group_id) + else: + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + for group_id in active_pose_groups: + group = groups.get(group_id) + if group is None or not bool(group.has_pose_target): + continue + handle_key = f"ee_control:{group_id}" + if handle_key in self._handles: + continue + ee_control = self.scene.ensure_target_controls( + group_id, + cast( + "Callable[[TransformControlsHandle], None]", + lambda target, gid=group_id: self._on_transform_update(gid, target), + ), + ) + if ee_control is not None: + self._handles[handle_key] = ee_control + pose = self.state.pose_targets.get(group_id) + if pose is not None: + self._suppress_target_callbacks = True + try: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False def _build_joint_sliders(self) -> None: - if self.state.selected_robot is None: - return gui = self.server.gui - config = self.adapter.get_robot_config(self.state.selected_robot) - if config is None: - return - current = self.adapter.get_current_joint_state(self.state.selected_robot) - values = list(current.position) if current is not None else [0.0] * len(config.joint_names) self._clear_joint_sliders() - joint_limits_lower = config.joint_limits_lower - joint_limits_upper = config.joint_limits_upper - for index, joint_name in enumerate(config.joint_names): - lower, upper = DEFAULT_JOINT_LIMITS - if joint_limits_lower is not None and index < len(joint_limits_lower): - lower = joint_limits_lower[index] - if joint_limits_upper is not None and index < len(joint_limits_upper): - upper = joint_limits_upper[index] - handle = gui.add_slider( - joint_name, - min=float(lower), - max=float(upper), - step=0.001, - initial_value=float(values[index] if index < len(values) else 0.0), + if not self.state.selected_group_ids: + return + joint_folder = self._handles.get("joint_control_folder") + if joint_folder is not None: + folder = cast("GuiFolderHandle", joint_folder) + with folder: + self._build_joint_slider_handles(gui) + return + self._build_joint_slider_handles(gui) + + def _build_joint_slider_handles(self, gui: GuiApi) -> None: + groups = self._group_info_by_id() + target_by_name: dict[str, float] = {} + if self.state.target_joints is not None: + target_by_name.update( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) ) + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + config = self.manipulation_module.get_robot_config(str(group.robot_name)) + current = self._get_current_joint_state(str(group.robot_name)) + current_by_name = joint_values_by_name(str(group.robot_name), current) + joint_limits_lower = config.joint_limits_lower if config is not None else None + joint_limits_upper = config.joint_limits_upper if config is not None else None + for index, (global_name, local_name) in enumerate( + zip(group.joint_names, group.local_joint_names, strict=False) + ): + joint_name = str(global_name) + local = str(local_name) + value = float( + target_by_name.get( + joint_name, + target_by_name.get( + local, current_by_name.get(joint_name, current_by_name.get(local, 0.0)) + ), + ) + ) + lower, upper = DEFAULT_JOINT_LIMITS + if joint_limits_lower is not None and index < len(joint_limits_lower): + lower = joint_limits_lower[index] + if joint_limits_upper is not None and index < len(joint_limits_upper): + upper = joint_limits_upper[index] + handle = gui.add_slider( + f"{group_id}/{local}", + min=float(lower), + max=float(upper), + step=0.001, + initial_value=value, + ) - def on_update(_event: object, name: str = joint_name) -> None: - self._on_joint_slider_update(name) - - handle.on_update(on_update) - self._joint_sliders[joint_name] = handle + def on_update(_event: object, name: str = joint_name) -> None: + self._on_joint_slider_update(name) + + handle.on_update(on_update) + self._joint_sliders[joint_name] = handle + + def _target_set_from_sliders(self) -> dict[PlanningGroupID, JointState] | None: + groups = self._group_info_by_id() + targets: dict[PlanningGroupID, JointState] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + self._set_error(f"Unknown planning group: {group_id}") + return None + names = [str(name) for name in group.joint_names] + positions: list[float] = [] + for name in names: + handle = self._joint_sliders.get(name) + if handle is None: + self._set_error(f"Missing target slider for {name}") + return None + positions.append(float(handle.value)) + targets[group_id] = JointState({"name": names, "position": positions}) + return targets + + def _split_target_joints_by_group(self, target_joints: JointState) -> None: + groups = self._group_info_by_id() + positions_by_name = dict(zip(target_joints.name, target_joints.position, strict=False)) + self.state.group_joint_targets.clear() + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + names = [str(name) for name in group.joint_names] + if not all(name in positions_by_name for name in names): + continue + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": [float(positions_by_name[name]) for name in names]} + ) def _clear_joint_sliders(self) -> None: for handle in self._joint_sliders.values(): @@ -284,51 +486,219 @@ def _remove_panel_handles(self) -> None: remove() self._handles.pop(key, None) - def _select_robot(self, robot_name: str) -> None: + def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: + seen_keys: set[str] = set() + selected = set(self.state.selected_group_ids) + for group in sorted( + groups, key=lambda item: (not bool(item.has_pose_target), str(item.id)) + ): + group_id = str(group.id) + key = f"group:{group_id}" + seen_keys.add(key) + handle = self._handles.get(key) + is_selected = group_id in selected + label = group_display_name(group) + if handle is None: + handle = self.server.gui.add_button( + label, + color=group_selector_color( + is_selected, ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR + ), + hint="Click to toggle this planning group in the target set.", + ) + handle.on_click( + cast( + "Callable[[object], None]", + lambda _event, gid=group_id: self._toggle_group_selected(gid), + ) + ) + self._handles[key] = handle + else: + self._set_optional_handle_attr(handle, "label", label) + self._set_optional_handle_attr( + handle, + "color", + group_selector_color(is_selected, ACTIVE_GROUP_COLOR, INACTIVE_GROUP_COLOR), + ) + + for key in [key for key in self._handles if key.startswith("group:")]: + if key not in seen_keys: + handle = self._handles.pop(key) + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + + def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None: + current = list(self.state.selected_group_ids) + if selected and group_id not in current: + current.append(group_id) + elif not selected and group_id in current: + current.remove(group_id) + self.state.selected_group_ids = tuple(current) + self._sync_group_selection_state() + self._prune_inactive_group_state() + self._initialize_selected_group_targets() + self.state.mark_plan_stale() + self._build_joint_sliders() + self.refresh() + + def _toggle_group_selected(self, group_id: PlanningGroupID) -> None: + self._set_group_selected(group_id, group_id not in self.state.selected_group_ids) + + def _select_all_manipulators(self) -> None: + groups = self._list_planning_groups() + manipulator_groups = [ + str(group.id) for group in groups if str(group.group_name) == "manipulator" + ] + self.state.selected_group_ids = tuple( + manipulator_groups or [str(group.id) for group in groups] + ) + self._sync_group_selection_state() + self._initialize_selected_group_targets() + self._build_joint_sliders() + self.refresh() + + def _clear_group_selection(self) -> None: if self._closed: return - if (robot_name or None) == self.state.selected_robot: - self.refresh() - return - self.state.selected_robot = robot_name or None + self.state.selected_group_ids = () + self._sync_group_selection_state() + self._prune_inactive_group_state() self.state.target_status = TargetStatus.EMPTY self.state.feasibility.status = FeasibilityStatus.UNKNOWN self.state.plan_state = PanelPlanState() self._build_joint_sliders() - self._sync_preset_dropdown() self.refresh() - def _sync_robot_dropdown(self, robots: list[str]) -> None: - handle = self._handles.get("robot") - if handle is None: - return - options = robots or [""] - for attr in ("options", "values"): - if hasattr(handle, attr): - try: - self._set_optional_handle_attr(handle, attr, options) - except Exception: - logger.warning("Could not set robot dropdown %s", attr, exc_info=True) - if hasattr(handle, "value") and self.state.selected_robot in robots: - try: - self._set_optional_handle_attr(handle, "value", self.state.selected_robot) - except Exception: - logger.warning("Could not set robot dropdown value", exc_info=True) + def _group_info_by_id(self) -> dict[PlanningGroupID, PlanningGroup]: + return {str(group.id): group for group in self._list_planning_groups()} + + def _sync_selected_robot_from_groups(self) -> None: + groups = self._group_info_by_id() + first_group = ( + groups.get(self.state.selected_group_ids[0]) if self.state.selected_group_ids else None + ) + self.state.selected_robot = None if first_group is None else str(first_group.robot_name) + + def _sync_group_selection_state(self) -> None: + self._sync_selected_robot_from_groups() + self.state.auxiliary_group_ids = self._selected_auxiliary_group_ids() + + def _selected_pose_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and bool(group.has_pose_target) + ) + + def _selected_auxiliary_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and not bool(group.has_pose_target) + ) + + def _active_pose_targets(self) -> dict[PlanningGroupID, Pose]: + return { + group_id: self.state.pose_targets[group_id] + for group_id in self._selected_pose_group_ids() + if group_id in self.state.pose_targets + } + + def _prune_inactive_group_state(self) -> None: + selected = set(self.state.selected_group_ids) + for mapping in ( + self.state.pose_targets, + self.state.group_joint_targets, + self.state.group_poses, + self.state.group_diagnostics, + ): + for group_id in [group_id for group_id in mapping if group_id not in selected]: + mapping.pop(group_id, None) + self._refresh_target_joints_from_groups() + + def _initialize_selected_group_targets(self) -> None: + groups = self._group_info_by_id() + for group_id in self.state.selected_group_ids: + if group_id in self.state.group_joint_targets: + continue + group = groups.get(group_id) + if group is None: + continue + current = self._get_current_joint_state(str(group.robot_name)) + if current is None: + continue + current_by_name = joint_values_by_name(str(group.robot_name), current) + names = [str(name) for name in group.joint_names] + local_names = [str(name) for name in group.local_joint_names] + positions = [ + float(current_by_name.get(global_name, current_by_name.get(local, 0.0))) + for global_name, local in zip(names, local_names, strict=False) + ] + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": positions} + ) + if bool(group.has_pose_target) and group_id not in self.state.pose_targets: + pose = self._get_ee_pose(str(group.robot_name)) + if pose is not None: + self.state.pose_targets[group_id] = pose + self.state.group_poses[group_id] = pose + if self.state.cartesian_target is None: + self.state.cartesian_target = pose + self._refresh_target_joints_from_groups() + + def _refresh_target_joints_from_groups(self) -> None: + names: list[str] = [] + positions: list[float] = [] + for group_id in self.state.selected_group_ids: + target = self.state.group_joint_targets.get(group_id) + if target is None: + continue + names.extend(target.name) + positions.extend(target.position) + self.state.target_joints = ( + JointState({"name": names, "position": positions}) if names else None + ) + + def _current_snapshot_by_group(self) -> dict[PlanningGroupID, list[float]]: + groups = self._group_info_by_id() + snapshot: dict[PlanningGroupID, list[float]] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + current = self._get_current_joint_state(str(group.robot_name)) + if current is None: + continue + current_by_name = joint_values_by_name(str(group.robot_name), current) + snapshot[group_id] = [ + float( + current_by_name.get(str(global_name), current_by_name.get(str(local_name), 0.0)) + ) + for global_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=False + ) + ] + return snapshot def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") - if handle is None or self.state.selected_robot is None: + if handle is None: return - info: RobotInfo | None = self.adapter.get_robot_info(self.state.selected_robot) - config = self.adapter.get_robot_config(self.state.selected_robot) + selected_robot_names = self._selected_robot_names() options = ["Select preset..."] - if (info is not None and info["init_joints"] is not None) or self.adapter.get_init_joints( - self.state.selected_robot - ) is not None: + if any( + self._get_init_joints(robot_name) is not None for robot_name in selected_robot_names + ): options.append("Init") options.append("Current") - home_joints = config.home_joints if config is not None else None - if (info is not None and info["home_joints"] is not None) or home_joints is not None: + if any( + (config := self.manipulation_module.get_robot_config(robot_name)) is not None + and config.home_joints is not None + for robot_name in selected_robot_names + ): options.append("Home") for attr in ("options", "values"): if hasattr(handle, attr): @@ -340,27 +710,64 @@ def _sync_preset_dropdown(self) -> None: def _apply_preset(self, preset: str) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: + if preset not in {"Current", "Init", "Home"}: return - config = self.adapter.get_robot_config(robot_name) - if config is None: - return - if preset == "Current": - current = self.adapter.get_current_joint_state(robot_name) - values = list(current.position) if current is not None else [] - elif preset == "Init": - init = self.adapter.get_init_joints(robot_name) - values = list(init.position) if init is not None else [] - elif preset == "Home": - values = list(config.home_joints or []) - else: - return - self._set_slider_values(config.joint_names, values) - self.state.joint_target = [float(value) for value in values] + groups = [ + group + for group in self._list_planning_groups() + if group.id in self.state.selected_group_ids + ] + for group in groups: + robot_name = str(group.robot_name) + values_by_name = self._preset_values_by_name(preset, robot_name) + global_names = [str(name) for name in group.joint_names] + local_names = [str(name) for name in group.local_joint_names] + values = [ + float(values_by_name.get(local_name, values_by_name.get(global_name, 0.0))) + for local_name, global_name in zip(local_names, global_names, strict=False) + ] + self._set_slider_values(global_names, values) + self.state.joint_target = [float(handle.value) for handle in self._joint_sliders.values()] self._submit_joint_target_evaluation() self.refresh() + def _selected_robot_names(self) -> tuple[str, ...]: + groups = self._group_info_by_id() + names: list[str] = [] + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group.robot_name) + if robot_name not in names: + names.append(robot_name) + return tuple(names) + + def _preset_values_by_name(self, preset: str, robot_name: str) -> dict[str, float]: + if preset == "Current": + current = self._get_current_joint_state(robot_name) + if current is None: + return {} + return { + str(name): float(value) + for name, value in zip(current.name, current.position, strict=False) + } + if preset == "Init": + init = self._get_init_joints(robot_name) + if init is None: + return {} + return { + str(name): float(value) + for name, value in zip(init.name, init.position, strict=False) + } + config = self.manipulation_module.get_robot_config(robot_name) + if config is None: + return {} + return { + str(name): float(value) + for name, value in zip(config.joint_names, config.home_joints or [], strict=False) + } + def _set_slider_values(self, joint_names: list[str], values: list[float]) -> None: self._suppress_target_callbacks = True try: @@ -371,18 +778,6 @@ def _set_slider_values(self, joint_names: list[str], values: list[float]) -> Non finally: self._suppress_target_callbacks = False - def _target_from_sliders(self, robot_name: str) -> JointState | None: - config = self.adapter.get_robot_config(robot_name) - if config is None: - self._set_error("No robot config") - return None - values = [ - float(self._joint_sliders[name].value) - for name in config.joint_names - if name in self._joint_sliders - ] - return self.adapter.joints_from_values(config.joint_names, values) - def _on_joint_slider_update(self, _joint_name: str) -> None: if self._closed: return @@ -390,73 +785,104 @@ def _on_joint_slider_update(self, _joint_name: str) -> None: return self._submit_joint_target_evaluation() - def _on_transform_update(self, target: TransformControlsHandle) -> None: + def _on_transform_update( + self, group_id: PlanningGroupID, target: TransformControlsHandle + ) -> None: if self._closed: return - if self._suppress_target_callbacks or self.state.selected_robot is None: - return - pose = self._pose_from_transform_target(target) - if pose is None: + if self._suppress_target_callbacks or group_id not in self.state.selected_group_ids: return + pose = pose_from_transform_values(target.position.tolist(), target.wxyz.tolist()) self.state.cartesian_target = pose + self.state.pose_targets[group_id] = pose sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="cartesian", - robot_name=self.state.selected_robot, - pose=pose, + group_ids=self.state.selected_group_ids, + auxiliary_group_ids=self._selected_auxiliary_group_ids(), + pose_targets=self._active_pose_targets(), + check_collision=True, ) ) self.refresh() def _submit_joint_target_evaluation(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: - return - target = self._target_from_sliders(robot_name) - if target is None: + targets = self._target_set_from_sliders() + if targets is None: return - self.state.joint_target = list(target.position) - self._move_joint_target_visuals(robot_name, target) + self.state.group_joint_targets = targets + self._refresh_target_joints_from_groups() + self._move_joint_target_visuals() sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name=robot_name, - joints=target, + group_ids=self.state.selected_group_ids, + joint_targets=dict(targets), ) ) self.refresh() - def _move_joint_target_visuals(self, robot_name: str, target: JointState) -> None: + def _move_joint_target_visuals(self) -> None: """Optimistically move target visuals before collision/feasibility returns.""" - config = self.adapter.get_robot_config(robot_name) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and config is not None and robot_id is not None: - self.scene.set_target_joints(str(robot_id), config.joint_names, list(target.position)) - pose = self.adapter.get_ee_pose(robot_name, target) - if pose is not None: - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), pose) - finally: - self._suppress_target_callbacks = False + if self.scene is None: + return + groups = self._group_info_by_id() + for group_id, target in self.state.group_joint_targets.items(): + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group.robot_name) + robot_id = self.manipulation_module.robot_id_for_name(robot_name) + config = self.manipulation_module.get_robot_config(robot_name) + if robot_id is None or config is None: + continue + local_positions = dict(zip(target.name, target.position, strict=False)) + joints = [ + float(local_positions.get(str(global_name), 0.0)) + for global_name in group.joint_names + ] + self.scene.set_target_joints(str(robot_id), list(group.local_joint_names), joints) + + def _sync_target_ghost_visibility(self) -> None: + if self.scene is None: + return + active_robot_ids: set[str] = set() + groups = self._group_info_by_id() + for group_id in self._selected_pose_group_ids(): + group = groups.get(group_id) + if group is None: + continue + robot_id = self.manipulation_module.robot_id_for_name(str(group.robot_name)) + if robot_id is not None: + active_robot_ids.add(str(robot_id)) + set_target_active = getattr(self.scene, "set_target_active", None) + if not callable(set_target_active): + return + for _robot_name, robot_id, _config in self.manipulation_module.robot_items(): + set_target_active(str(robot_id), str(robot_id) in active_robot_ids) def _handle_target_evaluation_request( self, request: TargetEvaluationRequest - ) -> TargetEvaluation: + ) -> TargetEvaluation | TargetSetEvaluation: if request.source == "cartesian": - if request.pose is None: + if not request.pose_targets: return {"success": False, "status": "INVALID", "message": "No pose target"} - return self.adapter.evaluate_pose_target(request.pose, request.robot_name) - if request.joints is None: + return self._evaluate_pose_target_set( + request.pose_targets, + auxiliary_groups=request.auxiliary_group_ids, + seed=self.state.last_valid_target_joints, + check_collision=request.check_collision, + ) + if not request.joint_targets: return {"success": False, "status": "INVALID", "message": "No joint target"} - return self.adapter.evaluate_joint_target(request.joints, request.robot_name) + return self._evaluate_joint_target_set(request.joint_targets) def _apply_target_evaluation_result( - self, request: TargetEvaluationRequest, result: TargetEvaluation + self, request: TargetEvaluationRequest, result: TargetEvaluation | TargetSetEvaluation ) -> None: if self._closed: return @@ -464,68 +890,100 @@ def _apply_target_evaluation_result( return collision_free = bool(result.get("collision_free", False)) success = bool(result.get("success", False)) - self.state.feasibility.status = self._feasibility_status(result, success, collision_free) + self.state.feasibility.status = feasibility_status( + str(result.get("status", "")), success, collision_free + ) self.state.feasibility.message = str(result.get("message", "")) self.state.target_status = ( TargetStatus.FEASIBLE if success and collision_free else TargetStatus.INFEASIBLE ) self.state.error = "" if success and collision_free else self.state.feasibility.message - if request.source == "joints": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - if request.source == "cartesian": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - pose = result.get("ee_pose") - if isinstance(pose, Pose): - self.state.cartesian_target = pose + target_joints = result.get("target_joints") or result.get("joint_state") + if isinstance(target_joints, JointState): + self.state.target_joints = JointState(target_joints) + self._split_target_joints_by_group(target_joints) + if success and collision_free: + self.state.last_valid_target_joints = JointState(target_joints) + group_poses = result.get("group_poses", {}) + if isinstance(group_poses, dict): + self.state.group_poses = { + str(group_id): pose + for group_id, pose in group_poses.items() + if isinstance(pose, Pose) + } + if request.source == "joints" and isinstance(target_joints, JointState): + self._sync_pose_targets_from_group_poses() + group_diagnostics = result.get("group_diagnostics", {}) + if isinstance(group_diagnostics, dict): + self.state.group_diagnostics = { + str(group_id): str(message) for group_id, message in group_diagnostics.items() + } + if request.source == "cartesian" and isinstance(target_joints, JointState): self._sync_controls_from_targets() self._update_target_visual_state() self.refresh() def _sync_controls_from_targets(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: - return - config = self.adapter.get_robot_config(robot_name) - if config is not None and self.state.joint_target is not None: - self._set_slider_values(list(config.joint_names), list(self.state.joint_target)) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and robot_id is not None: - self.scene.set_target_joints( - str(robot_id), config.joint_names, self.state.joint_target - ) + if self.state.target_joints is not None: + positions_by_name = dict( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) + ) + self._set_slider_values(list(positions_by_name), list(positions_by_name.values())) + self._move_joint_target_visuals() # Do not write the Cartesian target back into the active transform # control here. The gizmo is the source of truth for Cartesian edits; # programmatic pose writes from delayed IK results can fight fast user # dragging and make the gizmo jump back. + def _sync_pose_targets_from_group_poses(self) -> None: + groups = self._group_info_by_id() + updated_group_ids: list[PlanningGroupID] = [] + for group_id, pose in self.state.group_poses.items(): + group = groups.get(group_id) + if group is None or not bool(group.has_pose_target): + continue + if group_id not in self._selected_pose_group_ids(): + continue + self.state.pose_targets[group_id] = pose + updated_group_ids.append(group_id) + first_group_id = next(iter(self._selected_pose_group_ids()), None) + if first_group_id is not None: + self.state.cartesian_target = self.state.pose_targets.get(first_group_id) + self._sync_scene_target_pose_controls(updated_group_ids) + + def _sync_scene_target_pose_controls(self, group_ids: list[PlanningGroupID]) -> None: + if self.scene is None: + return + self._suppress_target_callbacks = True + try: + for group_id in group_ids: + pose = self.state.pose_targets.get(group_id) + if pose is not None: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False + def _update_status_text(self) -> None: current = self.state.current_joints + status_label = self.state.error or self.state.module_state status = [ - "### Manipulation Panel", - f"Robot: `{self.state.selected_robot or 'none'}`", - f"Module: `{self.state.module_state}`", - f"Backend: `{self.state.backend_status.value}`", - f"Target: `{self.state.target_status.value}`", - f"Feasibility: `{self.state.feasibility.status.value}`", - f"Plan: `{self.state.plan_state.status.value}`", - f"Action: `{self.state.action_status.value}`", + "### Status", + f"**State:** {status_label}", + f"Target: `{self.state.target_status.value}` · Plan: `{self.state.plan_state.status.value}`", ] if self.state.selected_robot is not None: - status.append( - f"State stale: `{self.adapter.is_state_stale(self.state.selected_robot)}`" - ) + status.append(f"State stale: `{self._is_state_stale(self.state.selected_robot)}`") if current is not None: status.append(f"Current joints: `{[round(v, 3) for v in current]}`") if self.state.last_result: status.append(f"Last result: `{self.state.last_result}`") - if self.state.error: - status.append(f"Error: `{self.state.error}`") self._set_handle_value("status", "\n\n".join(status)) + def _update_target_summary(self) -> None: + self._set_handle_value( + "target_summary", f"Feasibility: `{self.state.feasibility.status.value}`" + ) + def _update_control_state(self) -> None: self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) @@ -536,24 +994,29 @@ def _update_control_state(self) -> None: and self.state.can_execute(self.config.current_match_tolerance) ), ) - self._set_disabled("cancel", not self.state.can_cancel()) + can_cancel = self.state.can_cancel() + self._set_disabled("cancel", not can_cancel) + self._set_visible("cancel", can_cancel) self._update_target_visual_state() def _update_target_visual_state(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - self.scene.set_target_visual_state( - str(robot_id), self.state.feasibility.status == FeasibilityStatus.FEASIBLE + update_target_visual_state( + self.scene, + self._group_info_by_id(), + self.state.selected_group_ids, + self.manipulation_module.robot_id_for_name, + self.state.feasibility.status == FeasibilityStatus.FEASIBLE, ) def _submit_plan(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: + if not self.state.selected_group_ids: + self._set_recoverable_error( + "Cannot plan until target is feasible and manipulation is idle" + ) return if not self.state.can_plan(): self._set_recoverable_error( @@ -567,28 +1030,32 @@ def operation() -> None: return self.state.action_status = ActionStatus.RUNNING self.state.plan_state.status = PlanStatus.PLANNING - if self.state.manipulation_state == "FAULT" and not self.adapter.reset(): + if self.state.manipulation_state == "FAULT" and not self._reset(): self.state.plan_state.status = PlanStatus.FAILED self._finish_operation("reset=False", clear_error=False, operation_id=operation_id) return - target = self._target_from_sliders(robot_name) - if target is None: + targets = self._target_set_from_sliders() + if targets is None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation( "plan_to_joints=False", clear_error=False, operation_id=operation_id ) return - ok = self.adapter.plan_to_joints(target, robot_name) + ok = self.manipulation_module.plan_to_joint_targets( + cast("Mapping[PlanningGroupID | PlanningGroup, JointState]", targets) + ) if not self._operation_is_current(operation_id): return if ok: - path = self.adapter.get_planned_path(robot_name) self.state.plan_state.status = PlanStatus.FRESH - self.state.plan_state.robot = robot_name - self.state.plan_state.target_joints = list(target.position) + self.state.plan_state.group_ids = self.state.selected_group_ids + self.state.plan_state.robot = self.state.selected_robot + self.state.plan_state.target_joints = list( + self.state.target_joints.position if self.state.target_joints else [] + ) self.state.plan_state.target_pose = self.state.cartesian_target - self.state.plan_state.start_joints_snapshot = list(self.state.current_joints or []) - self.state.plan_state.planned_path = path + self.state.plan_state.start_joints_snapshot = self._current_snapshot_by_group() + self.state.plan_state.planned_path = None else: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation(f"plan_to_joints={ok}", operation_id=operation_id) @@ -600,9 +1067,6 @@ def operation() -> None: def _submit_preview(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.state.can_preview(): self._set_recoverable_error("No fresh plan to preview") return @@ -612,7 +1076,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.PREVIEWING - ok = self.adapter.preview_path(robot_name) + ok = self.manipulation_module.preview_plan() self._finish_operation(f"preview={ok}", operation_id=operation_id) self._operation_worker.submit( @@ -624,9 +1088,6 @@ def operation() -> None: def _submit_execute(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.config.allow_plan_execute: self._set_recoverable_error( "Panel execution disabled; set allow_plan_execute=True to enable" @@ -644,7 +1105,7 @@ def operation() -> None: return self.state.action_status = ActionStatus.EXECUTING self.state.plan_state.status = PlanStatus.EXECUTING - ok = self.adapter.execute(robot_name) + ok = self.manipulation_module.execute() if not self._operation_is_current(operation_id): return if not ok: @@ -666,7 +1127,7 @@ def _submit_cancel(self) -> None: self._mark_cancelled_plan_state(cancelled_action) self._restart_operation_worker() try: - ok = self.adapter.cancel() + ok = self.manipulation_module.cancel() except Exception as e: self._set_operation_error(str(e), operation_id) return @@ -695,7 +1156,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.CLEARING_PLAN - ok = self.adapter.clear_planned_path() + ok = self.manipulation_module.clear_planned_path() if not self._operation_is_current(operation_id): return self.state.plan_state = PanelPlanState() @@ -745,31 +1206,22 @@ def _set_error(self, message: str) -> None: def _set_handle_value(self, key: str, value: str) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiMarkdownHandle): - self._set_optional_handle_attr(handle, "value", value) + if handle is None: + return + if hasattr(handle, "content") or hasattr(handle, "value"): + attr = "content" if hasattr(handle, "content") else "value" + self._set_optional_handle_attr(handle, attr, value) def _set_disabled(self, key: str, disabled: bool) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiButtonHandle): + if handle is not None and hasattr(handle, "disabled"): self._set_optional_handle_attr(handle, "disabled", disabled) + def _set_visible(self, key: str, visible: bool) -> None: + handle = self._handles.get(key) + if handle is not None: + self._set_optional_handle_attr(handle, "visible", visible) + @staticmethod def _set_optional_handle_attr(handle: object, attr: str, value: object) -> None: setattr(handle, attr, value) - - def _pose_from_transform_target(self, target: TransformControlsHandle) -> Pose | None: - px, py, pz = (float(value) for value in target.position) - qw, qx, qy, qz = (float(value) for value in target.wxyz) - return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) - - def _feasibility_status( - self, result: TargetEvaluation, success: bool, collision_free: bool - ) -> FeasibilityStatus: - status = str(result.get("status", "")).upper() - if success and collision_free: - return FeasibilityStatus.FEASIBLE - if status in {"COLLISION", "COLLISION_AT_START", "COLLISION_AT_GOAL"}: - return FeasibilityStatus.COLLISION - if status in {"NO_SOLUTION", "SINGULARITY", "JOINT_LIMITS", "TIMEOUT"}: - return FeasibilityStatus.IK_FAILED - return FeasibilityStatus.INVALID diff --git a/dimos/manipulation/visualization/viser/panel_backend.py b/dimos/manipulation/visualization/viser/panel_backend.py new file mode 100644 index 0000000000..6fbc51a408 --- /dev/null +++ b/dimos/manipulation/visualization/viser/panel_backend.py @@ -0,0 +1,268 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from typing import TYPE_CHECKING, cast + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName +from dimos.manipulation.visualization.types import TargetSetEvaluation +from dimos.manipulation.visualization.viser.state import FeasibilityStatus +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + +if TYPE_CHECKING: + from dimos.manipulation.manipulation_module import ManipulationModule + from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor + + +def copy_joint_state(joint_state: JointState | None) -> JointState | None: + return None if joint_state is None else JointState(joint_state) + + +def get_current_joint_state( + world_monitor: WorldMonitor, + manipulation_module: ManipulationModule, + robot_name: RobotName, +) -> JointState | None: + current = manipulation_module.get_current_joint_state(robot_name) + if current is None: + robot_id = manipulation_module.robot_id_for_name(robot_name) + if robot_id is not None: + current = world_monitor.get_current_joint_state(robot_id) + return copy_joint_state(current) + + +def is_state_stale( + world_monitor: WorldMonitor, + manipulation_module: ManipulationModule, + robot_name: RobotName, + max_age: float = 1.0, +) -> bool: + robot_id = manipulation_module.robot_id_for_name(robot_name) + return True if robot_id is None else world_monitor.is_state_stale(robot_id, max_age) + + +def get_ee_pose( + world_monitor: WorldMonitor, + manipulation_module: ManipulationModule, + groups: Sequence[PlanningGroup], + robot_name: RobotName, + joint_state: JointState | None = None, +) -> Pose | None: + group_id = primary_pose_group_id(groups, robot_name) + if group_id is None: + return None + copied = copy_joint_state(joint_state) + fk = manipulation_module.forward_kinematics(group_id, copied) + if fk.pose is not None: + return cast("Pose", fk.pose) + robot_id = manipulation_module.robot_id_for_name(robot_name) + if robot_id is None: + return None + get_world_ee_pose = getattr(world_monitor, "get_ee_pose", None) + if callable(get_world_ee_pose): + return cast("Pose | None", get_world_ee_pose(robot_id, copy_joint_state(joint_state))) + return None + + +def evaluate_joint_target_set( + manipulation_module: ManipulationModule, + joint_targets: Mapping[PlanningGroupID, JointState], +) -> TargetSetEvaluation: + if not joint_targets: + return {"success": False, "status": "INVALID", "message": "No joint target"} + names: list[str] = [] + positions: list[float] = [] + for target in joint_targets.values(): + copied = copy_joint_state(target) + if copied is None: + continue + names.extend(str(name) for name in copied.name) + positions.extend(float(position) for position in copied.position) + return evaluate_global_target_set( + manipulation_module, + tuple(joint_targets), + JointState({"name": names, "position": positions}), + ) + + +def evaluate_pose_target_set( + manipulation_module: ManipulationModule, + pose_targets: Mapping[PlanningGroupID, Pose], + auxiliary_groups: Sequence[PlanningGroupID] = (), + seed: JointState | None = None, + check_collision: bool = True, +) -> TargetSetEvaluation: + if not pose_targets: + return {"success": False, "status": "INVALID", "message": "No pose target"} + stamped_targets = {group_id: stamped_pose(pose) for group_id, pose in pose_targets.items()} + group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_groups))) + ik = manipulation_module.inverse_kinematics( + pose_targets=stamped_targets, + auxiliary_group_ids=auxiliary_groups, + seed=copy_joint_state(seed), + ) + if not ik.is_success() or ik.joint_state is None: + return { + "success": False, + "status": ik.status.name, + "message": ik.message, + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + "position_error": ik.position_error, + "orientation_error": ik.orientation_error, + } + return evaluate_global_target_set( + manipulation_module, + group_ids, + ik.joint_state, + status=ik.status.name, + message=ik.message, + position_error=ik.position_error, + orientation_error=ik.orientation_error, + check_collision=check_collision, + ) + + +def evaluate_global_target_set( + manipulation_module: ManipulationModule, + group_ids: tuple[PlanningGroupID, ...], + target_joints: JointState, + *, + status: str = "FEASIBLE", + message: str = "Target is collision-free", + position_error: float = 0.0, + orientation_error: float = 0.0, + check_collision: bool = True, +) -> TargetSetEvaluation: + target = copy_joint_state(target_joints) + if target is None: + return {"success": False, "status": "INVALID", "message": "No target joints"} + collision = manipulation_module.check_collision(target) if check_collision else None + collision_free = True if collision is None else collision.collision_free is True + diagnostics = { + group_id: "Target collision check skipped" if collision is None else collision.message + for group_id in group_ids + } + result_message = message + if collision is None: + result_message = "Target collision check skipped" + elif not collision_free: + result_message = collision.message + group_poses: dict[PlanningGroupID, Pose | None] = {} + for group_id in group_ids: + fk = manipulation_module.forward_kinematics(group_id, target) + if fk.pose is not None: + group_poses[group_id] = cast("Pose", fk.pose) + elif fk.status != "INVALID": + group_poses[group_id] = None + return { + "success": collision_free, + "status": status + if collision_free + else collision.status + if collision is not None + else "COLLISION", + "message": result_message, + "collision_free": collision_free, + "group_ids": group_ids, + "target_joints": target, + "group_diagnostics": diagnostics, + "group_poses": group_poses, + "position_error": position_error, + "orientation_error": orientation_error, + } + + +def primary_pose_group_id( + groups: Sequence[PlanningGroup], robot_name: RobotName +) -> PlanningGroupID | None: + for group in groups: + if group.robot_name == robot_name and group.has_pose_target: + return group.id + return None + + +def stamped_pose(pose: Pose | PoseStamped) -> PoseStamped: + if isinstance(pose, PoseStamped): + return pose + return PoseStamped(frame_id="world", position=pose.position, orientation=pose.orientation) + + +def joint_values_by_name(robot_name: str, joint_state: JointState | None) -> dict[str, float]: + if joint_state is None: + return {} + values: dict[str, float] = {} + for name, position in zip(joint_state.name, joint_state.position, strict=False): + name_str = str(name) + position_float = float(position) + values[name_str] = position_float + if "/" in name_str: + values[name_str.rsplit("/", 1)[1]] = position_float + else: + values[f"{robot_name}/{name_str}"] = position_float + return values + + +def pose_from_transform_values(position: Sequence[float], wxyz: Sequence[float]) -> Pose: + px, py, pz = (float(value) for value in position) + qw, qx, qy, qz = (float(value) for value in wxyz) + return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) + + +def feasibility_status( + status: str, + success: bool, + collision_free: bool, +) -> FeasibilityStatus: + normalized = status.upper() + if success and collision_free: + return FeasibilityStatus.FEASIBLE + if normalized in {"COLLISION", "COLLISION_AT_START", "COLLISION_AT_GOAL"}: + return FeasibilityStatus.COLLISION + if normalized in {"NO_SOLUTION", "SINGULARITY", "JOINT_LIMITS", "TIMEOUT"}: + return FeasibilityStatus.IK_FAILED + return FeasibilityStatus.INVALID + + +def update_target_visual_state( + scene: object, + groups: Mapping[PlanningGroupID, PlanningGroup], + selected_group_ids: Sequence[PlanningGroupID], + robot_id_for_name: Callable[[RobotName], object | None], + feasible: bool, +) -> None: + set_visual_state = getattr(scene, "set_target_visual_state", None) + if not callable(set_visual_state): + return + updated: set[str] = set() + for group_id in selected_group_ids: + group_id_str = str(group_id) + if group_id_str not in updated: + set_visual_state(group_id_str, feasible) + updated.add(group_id_str) + group = groups.get(group_id_str) + if group is None: + continue + robot_id = robot_id_for_name(str(group.robot_name)) + robot_id_str = None if robot_id is None else str(robot_id) + if robot_id_str is None or robot_id_str in updated: + continue + set_visual_state(robot_id_str, feasible) + updated.add(robot_id_str) diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index cd50ab098e..049ffdf659 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -16,11 +16,16 @@ from collections.abc import Callable, Sequence from pathlib import Path +import time from typing import Protocol, TypeAlias, cast from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.manipulation.visualization.viser.animation import PreviewAnimator +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, + sampled_joint_path_frames, +) from dimos.manipulation.visualization.viser.runtime import ( VISER_INSTALL_HINT, VISER_URDF_INSTALL_HINT, @@ -31,6 +36,7 @@ try: from viser import ( + FrameHandle, GridHandle, MeshHandle, TransformControlsEvent, @@ -69,7 +75,7 @@ REFERENCE_GRID_CELL_COLOR = (44, 54, 58) REFERENCE_GRID_SECTION_COLOR = (90, 145, 165) -SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle +SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle | FrameHandle class _ColorHandle(Protocol): @@ -88,9 +94,11 @@ def __init__( self._configs_by_id: dict[str, RobotModelConfig] = {} self._urdfs: dict[str, ViserUrdf] = {} self._handles: dict[str, TransformControlsHandle] = {} + self._root_frames: dict[str, FrameHandle] = {} self._grid_handle: GridHandle | None = None self._grid_visible = True self._preview_visible: dict[str, bool] = {} + self._target_active: dict[str, bool] = {} self._target_tracks_current: dict[str, bool] = {} self._ensure_reference_grid() @@ -106,9 +114,17 @@ def set_reference_grid_visible(self, visible: bool) -> None: def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: self._configs_by_id[robot_id] = config self._preview_visible.setdefault(robot_id, False) + self._target_active.setdefault(robot_id, False) self._target_tracks_current.setdefault(robot_id, True) self._ensure_robot_urdfs(robot_id, config) + def set_target_active(self, robot_id: str, active: bool) -> None: + """Show target ghost only when at least one group on the robot is active.""" + self._target_active[robot_id] = active + if not active: + self._target_tracks_current[robot_id] = True + self._set_target_visibility(robot_id, active) + def _ensure_reference_grid(self) -> None: try: scene = self.server.scene @@ -155,6 +171,9 @@ def dispatch(event: TransformControlsEvent) -> None: self._handles[handle_key] = handle return handle + def remove_target_controls(self, robot_id: str) -> None: + self._remove_handle(f"{robot_id}:ee_control") + def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> None: config = self._configs_by_id.get(robot_id) if config is None or joint_state is None: @@ -164,7 +183,7 @@ def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> self.set_urdf_joints(current, config.joint_names, joint_state.position) if self._target_tracks_current.get(robot_id, True): self._set_target_joints(robot_id, config.joint_names, joint_state.position) - self._set_target_visibility(robot_id, True) + self._set_target_visibility(robot_id, self._target_active.get(robot_id, False)) def show_preview(self, robot_id: str) -> None: """Show the transient preview-animation ghost. @@ -183,13 +202,63 @@ def animate_path(self, robot_id: str, path: Sequence[JointState], duration: floa config = self._configs_by_id.get(robot_id) if config is None: return False - self.show_preview(robot_id) + preview = GroupPreviewAnimation( + group_ids=(), + tracks=( + PreviewTrack( + robot_id=robot_id, + group_ids=(), + joint_names=tuple(config.joint_names), + path=tuple(path), + ), + ), + ) + return self.animate_preview(preview, duration) + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> bool: + """Animate all preview tracks with one shared group-native frame clock.""" + if not preview.tracks: + return False + frames_by_robot: dict[str, list[list[float]]] = {} + joint_names_by_robot: dict[str, tuple[str, ...]] = {} + for track in preview.tracks: + if track.robot_id not in self._configs_by_id: + return False + frames = sampled_joint_path_frames(track.path, duration, self.preview_fps) + if not frames: + return False + frames_by_robot[track.robot_id] = frames + joint_names_by_robot[track.robot_id] = track.joint_names + + frame_count = max(len(frames) for frames in frames_by_robot.values()) + if frame_count <= 0: + return False + step_delay = duration / max(frame_count - 1, 1) if duration > 0.0 else 0.0 + + robot_ids = tuple(frames_by_robot) + for robot_id in robot_ids: + self.show_preview(robot_id) try: - return PreviewAnimator( - lambda joints: self._set_preview_ghost_joints(robot_id, config.joint_names, joints) - ).animate(path, duration, self.preview_fps) + for frame_index in range(frame_count): + for robot_id in robot_ids: + frames = frames_by_robot[robot_id] + joints = self._frame_at_shared_index(frames, frame_index, frame_count) + self._set_preview_ghost_joints(robot_id, joint_names_by_robot[robot_id], joints) + if frame_index < frame_count - 1: + time.sleep(step_delay) + return True finally: - self.hide_preview(robot_id) + for robot_id in robot_ids: + self.hide_preview(robot_id) + + @staticmethod + def _frame_at_shared_index( + frames: Sequence[list[float]], frame_index: int, frame_count: int + ) -> list[float]: + if frame_count <= 1 or len(frames) == 1: + return frames[-1] + source_index = round(frame_index * (len(frames) - 1) / (frame_count - 1)) + return frames[source_index] def set_target_joints( self, robot_id: str, joint_names: Sequence[str], joints: Sequence[float] @@ -197,6 +266,7 @@ def set_target_joints( target = self._urdfs.get(f"{robot_id}:target") if target is None: return False + self._target_active[robot_id] = True self._target_tracks_current[robot_id] = False self._set_target_joints(robot_id, joint_names, joints) self._set_target_visibility(robot_id, True) @@ -252,9 +322,13 @@ def close(self) -> None: self._grid_handle = None for urdf in self._urdfs.values(): self._remove_scene_handle(urdf) + for frame in self._root_frames.values(): + self._remove_scene_handle(frame) self._urdfs.clear() + self._root_frames.clear() self._configs_by_id.clear() self._preview_visible.clear() + self._target_active.clear() self._target_tracks_current.clear() def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: @@ -264,11 +338,7 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: key = f"{robot_id}:{kind}" if key in self._urdfs: continue - root_node_name = { - "current": f"/robots/{robot_id}/current", - "target": f"/targets/{robot_id}/target", - "preview": f"/previews/{robot_id}/ghost", - }[kind] + root_node_name = self._urdf_root_node_name(robot_id, kind, config) mesh_color_override = { "current": None, "target": GOAL_ROBOT_MESH_COLOR, @@ -284,7 +354,9 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: self._set_urdf_mesh_material( self._urdfs[key], GOAL_ROBOT_FEASIBLE_COLOR, GOAL_ROBOT_FEASIBLE_OPACITY ) - self._set_handle_visibility(self._urdfs[key], True) + self._set_handle_visibility( + self._urdfs[key], self._target_active.get(robot_id, False) + ) elif kind == "preview": self._set_urdf_mesh_material( self._urdfs[key], PREVIEW_ROBOT_COLOR, PREVIEW_ROBOT_OPACITY @@ -301,6 +373,64 @@ def prepared_urdf_path(self, config: RobotModelConfig) -> Path: package_paths=package_paths, xacro_args={str(key): str(value) for key, value in config.xacro_args.items()}, convert_meshes=bool(config.auto_convert_meshes), + strip_world_joint_child_link=str(config.base_link) + if bool(getattr(config, "strip_model_world_joint", False)) + else None, + ) + ) + + def _urdf_root_node_name(self, robot_id: str, kind: str, config: RobotModelConfig) -> str: + root_node_name = { + "current": f"/robots/{robot_id}/current", + "target": f"/targets/{robot_id}/target", + "preview": f"/previews/{robot_id}/ghost", + }[kind] + if not self._has_non_identity_base_pose(config): + return root_node_name + self._ensure_base_pose_frame(robot_id, kind, config) + return f"{root_node_name}/base_pose/urdf" + + def _ensure_base_pose_frame(self, robot_id: str, kind: str, config: RobotModelConfig) -> None: + key = f"{robot_id}:{kind}:base_pose" + if key in self._root_frames: + return + pose = config.base_pose + frame_name = { + "current": f"/robots/{robot_id}/current/base_pose", + "target": f"/targets/{robot_id}/target/base_pose", + "preview": f"/previews/{robot_id}/ghost/base_pose", + }[kind] + self._root_frames[key] = self.server.scene.add_frame( + frame_name, + show_axes=False, + position=( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + ), + wxyz=( + float(pose.orientation.w), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + ), + ) + + @staticmethod + def _has_non_identity_base_pose(config: RobotModelConfig) -> bool: + pose = getattr(config, "base_pose", None) + if pose is None: + return False + return any( + abs(value) > 1e-12 + for value in ( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + float(pose.orientation.w) - 1.0, ) ) diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index b46097df95..f16e490ffb 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -21,7 +21,8 @@ import threading from typing import Literal -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -92,16 +93,25 @@ class FeasibilityState: @dataclass class PanelPlanState: status: PlanStatus = PlanStatus.NONE + group_ids: tuple[PlanningGroupID, ...] = () robot: str | None = None target_pose: Pose | None = None target_joints: list[float] | None = None - start_joints_snapshot: list[float] | None = None + start_joints_snapshot: dict[PlanningGroupID, list[float]] = field(default_factory=dict) planned_path: list[JointState] | None = None @dataclass class PanelState: selected_robot: str | None = None + selected_group_ids: tuple[PlanningGroupID, ...] = () + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + target_joints: JointState | None = None + last_valid_target_joints: JointState | None = None + group_poses: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_diagnostics: dict[PlanningGroupID, str] = field(default_factory=dict) runtime: PanelRuntime = PanelRuntime.STOPPED backend_status: BackendConnectionStatus = BackendConnectionStatus.DISCONNECTED target_status: TargetStatus = TargetStatus.EMPTY @@ -133,9 +143,10 @@ def can_plan(self) -> bool: return ( self.runtime == PanelRuntime.RUNNING and self.backend_status == BackendConnectionStatus.READY - and self.selected_robot is not None + and bool(self.selected_group_ids) and self.action_status == ActionStatus.IDLE and self.target_status == TargetStatus.FEASIBLE + and self.target_joints is not None and self.manipulation_state in {"IDLE", "COMPLETED", "FAULT"} and self.plan_state.status != PlanStatus.PLANNING ) @@ -169,19 +180,24 @@ def can_execute( and self.target_status == TargetStatus.FEASIBLE and self.manipulation_state in {"IDLE", "COMPLETED"} and plan.status == PlanStatus.FRESH - and plan.robot == self.selected_robot - and plan.start_joints_snapshot is not None - and self.current_joints is not None + and plan.group_ids == self.selected_group_ids + and bool(plan.start_joints_snapshot) + and self.target_joints is not None ): return False - if len(plan.start_joints_snapshot) != len(self.current_joints): + if self.current_joints is None: return False - return all( - abs(expected - current) <= current_tolerance - for expected, current in zip( - plan.start_joints_snapshot, self.current_joints, strict=False + # Multi-group freshness is checked by group snapshot when available. The + # robot-level current-joint fallback preserves one-group legacy tests. + if len(plan.start_joints_snapshot) == 1: + snapshot = next(iter(plan.start_joints_snapshot.values())) + if len(snapshot) != len(self.current_joints): + return False + return all( + abs(expected - current) <= current_tolerance + for expected, current in zip(snapshot, self.current_joints, strict=False) ) - ) + return True @property def connected(self) -> bool: @@ -203,9 +219,14 @@ def module_state(self) -> str: class TargetEvaluationRequest: sequence_id: int source: PreviewSource - robot_name: str + group_ids: tuple[PlanningGroupID, ...] + robot_name: str | None = None + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () pose: Pose | None = None + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) joints: JointState | None = None + joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + check_collision: bool = True class TargetEvaluationWorker: @@ -218,8 +239,10 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest], TargetEvaluation], - apply_result: Callable[[TargetEvaluationRequest, TargetEvaluation], None], + handler: Callable[[TargetEvaluationRequest], TargetEvaluation | TargetSetEvaluation], + apply_result: Callable[ + [TargetEvaluationRequest, TargetEvaluation | TargetSetEvaluation], None + ], ) -> None: self._handler = handler self._apply_result = apply_result diff --git a/dimos/manipulation/visualization/viser/test_gui_status.py b/dimos/manipulation/visualization/viser/test_gui_status.py index f461565bdf..617f32a988 100644 --- a/dimos/manipulation/visualization/viser/test_gui_status.py +++ b/dimos/manipulation/visualization/viser/test_gui_status.py @@ -18,46 +18,28 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.visualization.types import TargetEvaluation -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter -from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.gui import ViserPanelGui +from dimos.manipulation.visualization.viser.panel_backend import feasibility_status from dimos.manipulation.visualization.viser.state import FeasibilityStatus -class StatusOnlyServer: - pass - - -class StatusOnlyAdapter(InProcessViserAdapter): - def __init__(self) -> None: - pass - - @pytest.mark.parametrize( - ("result", "success", "collision_free", "expected"), + ("status", "success", "collision_free", "expected"), [ - ({"status": "FEASIBLE"}, True, True, FeasibilityStatus.FEASIBLE), - ({"status": "COLLISION"}, False, False, FeasibilityStatus.COLLISION), - ({"status": "COLLISION_AT_START"}, False, False, FeasibilityStatus.COLLISION), - ({"status": "COLLISION_AT_GOAL"}, False, False, FeasibilityStatus.COLLISION), - ({"status": "NO_SOLUTION"}, False, False, FeasibilityStatus.IK_FAILED), - ({"status": "SINGULARITY"}, False, False, FeasibilityStatus.IK_FAILED), - ({"status": "JOINT_LIMITS"}, False, False, FeasibilityStatus.IK_FAILED), - ({"status": "TIMEOUT"}, False, False, FeasibilityStatus.IK_FAILED), - ({"status": "IK_SUCCEEDED"}, False, False, FeasibilityStatus.INVALID), + ("FEASIBLE", True, True, FeasibilityStatus.FEASIBLE), + ("COLLISION", False, False, FeasibilityStatus.COLLISION), + ("COLLISION_AT_START", False, False, FeasibilityStatus.COLLISION), + ("COLLISION_AT_GOAL", False, False, FeasibilityStatus.COLLISION), + ("NO_SOLUTION", False, False, FeasibilityStatus.IK_FAILED), + ("SINGULARITY", False, False, FeasibilityStatus.IK_FAILED), + ("JOINT_LIMITS", False, False, FeasibilityStatus.IK_FAILED), + ("TIMEOUT", False, False, FeasibilityStatus.IK_FAILED), + ("IK_SUCCEEDED", False, False, FeasibilityStatus.INVALID), ], ) def test_gui_feasibility_status_uses_exact_status_mapping( - result: TargetEvaluation, + status: str, success: bool, collision_free: bool, expected: FeasibilityStatus, ) -> None: - gui = ViserPanelGui( - StatusOnlyServer(), - StatusOnlyAdapter(), - ViserVisualizationConfig(), - ) - - assert gui._feasibility_status(result, success, collision_free) == expected + assert feasibility_status(status, success, collision_free) == expected diff --git a/dimos/manipulation/visualization/viser/test_operation_worker.py b/dimos/manipulation/visualization/viser/test_operation_worker.py index ee1d5c29c0..8367d05a19 100644 --- a/dimos/manipulation/visualization/viser/test_operation_worker.py +++ b/dimos/manipulation/visualization/viser/test_operation_worker.py @@ -22,8 +22,8 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.visualization.types import RobotInfo -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.state import ( @@ -42,6 +42,14 @@ class EmptyServer: pass +class EmptyWorldMonitor: + def get_current_joint_state(self, robot_id: str) -> None: + return None + + def is_state_stale(self, robot_id: str, max_age: float = 1.0) -> bool: + return False + + @dataclass class FakeStopOperationWorker(OperationWorker): stop_calls: list[float | None] @@ -106,19 +114,25 @@ def stop(self, timeout: float | None = 2.0) -> None: self.stop_calls.append(timeout) -class FakeOperationAdapter(InProcessViserAdapter): +class FakeOperationAdapter: def __init__(self) -> None: self.cancel_calls = 0 def list_robots(self) -> list[str]: return [] - def get_module_state(self) -> str: + def robot_id_for_name(self, robot_name: str) -> str | None: + return None + + def get_state(self) -> str: return "IDLE" def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None + def list_planning_groups(self) -> list[PlanningGroup]: + return [] + def get_current_joint_state(self, robot_name: str) -> None: return None @@ -141,6 +155,12 @@ def cancel(self) -> bool: def plan_to_joints(self, joints: JointState, robot_name: str | None = None) -> bool: return True + def robot_items(self) -> list[tuple[str, str, object]]: + return [] + + def plan_to_joint_targets(self, joint_targets: dict[str, JointState]) -> bool: + return True + def test_operation_worker_uses_per_operation_timeout() -> None: errors: list[str] = [] @@ -181,6 +201,7 @@ def test_gui_close_uses_bounded_operation_worker_stop(monkeypatch: pytest.Monkey stop_timeouts: list[float | None] = [] gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) @@ -198,6 +219,7 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat submissions: list[dict[str, float]] = [] gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(preview_request_timeout=0.25), ) @@ -205,7 +227,8 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat monkeypatch.setattr(gui, "_operation_worker", FakeTimeoutSubmitWorker(submissions)) gui.state.runtime = PanelRuntime.RUNNING gui.state.backend_status = BackendConnectionStatus.READY - gui.state.selected_robot = "arm" + gui.state.selected_group_ids = ("arm:manipulator",) + gui.state.target_joints = JointState({"name": ["arm/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -223,6 +246,7 @@ def test_gui_cancel_bypasses_operation_worker(monkeypatch: pytest.MonkeyPatch) - adapter = FakeOperationAdapter() gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), adapter, ViserVisualizationConfig(), ) @@ -248,6 +272,7 @@ def test_gui_cancelled_planning_clears_active_plan_state(monkeypatch: pytest.Mon adapter = FakeOperationAdapter() gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), adapter, ViserVisualizationConfig(), ) @@ -286,6 +311,7 @@ def test_gui_guard_errors_keep_action_idle( submissions: list[Callable[[], None]] = [] gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) @@ -306,6 +332,7 @@ def test_gui_guard_errors_keep_action_idle( def test_gui_ignores_stale_timed_out_operation_finish() -> None: gui = ViserPanelGui( EmptyServer(), + EmptyWorldMonitor(), FakeOperationAdapter(), ViserVisualizationConfig(), ) diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index 412c227050..040ae6f56e 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -20,11 +20,14 @@ PanelState, TargetStatus, ) +from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_can_plan_from_fault_after_planning_failure() -> None: state = PanelState( selected_robot="arm", + selected_group_ids=("arm:manipulator",), + target_joints=JointState({"name": ["arm/j1"], "position": [1.0]}), runtime=PanelRuntime.RUNNING, backend_status=BackendConnectionStatus.READY, target_status=TargetStatus.FEASIBLE, diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 3be6162a67..42d78c068f 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -25,15 +25,30 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.spec.enums import IKStatus +from dimos.manipulation.planning.spec.models import ( + CollisionCheckResult, + ForwardKinematicsResult, + IKResult, +) +from dimos.manipulation.visualization.types import RobotInfo +from dimos.manipulation.visualization.viser import scene as scene_module from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, PreviewAnimator, + PreviewTrack, interpolate_joint_path, sampled_joint_path_frames, ) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.gui import ViserPanelGui +from dimos.manipulation.visualization.viser.gui import ( + ACTIVE_GROUP_COLOR, + INACTIVE_GROUP_COLOR, + PRIMARY_ACTION_COLOR, + ViserPanelGui, +) +from dimos.manipulation.visualization.viser.panel_backend import pose_from_transform_values from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( ActionStatus, @@ -47,11 +62,15 @@ ) from dimos.manipulation.visualization.viser.theme import _dimos_logo_data_url, apply_dimos_theme from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState GuiCallback = Callable[[SimpleNamespace], None] ThemeValue = str | bool | tuple[int, int, int] | dict[str, str | dict[str, str]] | None RobotConfigOverride = str | list[str] | list[float] | None +DEFAULT_GROUP_ID = "arm:manipulator" @dataclass @@ -120,6 +139,7 @@ def remove(self) -> None: class GuiButtonHandle: label: str disabled: bool = False + color: tuple[int, int, int] | None = None click_callback: GuiCallback | None = None removed: bool = False @@ -166,7 +186,7 @@ def __init__(self) -> None: self.visible: object | None = None self.removed = False self.name = "" - self.kwargs: dict[str, float | bool] = {} + self.kwargs: dict[str, object] = {} def remove(self) -> None: self.removed = True @@ -206,6 +226,8 @@ class FakeServer: def __init__(self) -> None: self.scene = SimpleNamespace() self.scene.add_transform_controls = self.add_transform_controls + self.scene.add_frame = self.add_frame + self.frames = [] def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHandle: handle = FakeTransformHandle() @@ -213,6 +235,13 @@ def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHan handle.scale = scale return handle + def add_frame(self, name: str, **kwargs: object) -> FakeHandle: + handle = FakeHandle() + handle.name = name + handle.kwargs = kwargs + self.frames.append(handle) + return handle + class FakeGridServer(FakeServer): def __init__(self) -> None: @@ -223,7 +252,7 @@ def __init__(self) -> None: def add_grid(self, name: str, **kwargs: float | bool) -> FakeHandle: handle = FakeHandle() handle.name = name - handle.kwargs = kwargs + handle.kwargs = dict(kwargs) handle.visible = kwargs.get("visible") self.grids.append(handle) return handle @@ -314,8 +343,16 @@ def add_dropdown( handle = GuiDropdownHandle(label=label, options=list(options), value=initial_value) return handle - def add_button(self, label: str, *, disabled: bool = False) -> GuiButtonHandle: - handle = GuiButtonHandle(label=label, disabled=disabled) + def add_button( + self, + label: str, + *, + disabled: bool = False, + color: tuple[int, int, int] | None = None, + hint: str | None = None, + ) -> GuiButtonHandle: + _ = hint + handle = GuiButtonHandle(label=label, disabled=disabled, color=color) self.buttons[label] = handle return handle @@ -346,8 +383,28 @@ def make_robot_config(**overrides: RobotConfigOverride) -> RobotConfigStub: return config +def make_planning_group_info( + robot_name: str, + config: RobotConfigStub | SimpleNamespace, + *, + group_name: str = "manipulator", + has_pose_target: bool = True, +) -> PlanningGroup: + joint_names = [str(name) for name in config.joint_names] + return PlanningGroup( + id=f"{robot_name}:{group_name}", + group_name=group_name, + robot_name=robot_name, + joint_names=tuple(f"{robot_name}/{name}" for name in joint_names), + local_joint_names=tuple(joint_names), + base_link=str(config.base_link), + tip_link=str(config.end_effector_link) if has_pose_target else None, + source="fallback", + ) + + class FakeManipulationModule(SimpleNamespace): - """Public ManipulationModule surface used by the in-process Viser adapter tests.""" + """Public ManipulationModule surface used by the in-process Viser panel tests.""" def list_robots(self) -> list[str]: return list(getattr(self, "_robots", {}).keys()) @@ -378,10 +435,16 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None init = self.get_init_joints(robot_name) home_joints = config.home_joints if hasattr(config, "home_joints") else None + planning_groups = getattr(self, "_planning_groups", None) + if planning_groups is None: + planning_groups = [make_planning_group_info(robot_name, config)] + else: + planning_groups = [group for group in planning_groups if group.robot_name == robot_name] return { "name": config.name, "world_robot_id": self.robot_id_for_name(robot_name) or robot_name, "joint_names": list(config.joint_names), + "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": 1.0, @@ -393,16 +456,17 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: "init_joints": list(init.position) if init is not None else None, } + def list_planning_groups(self) -> list[PlanningGroup]: + groups: list[PlanningGroup] = [] + for robot_name in self.list_robots(): + info = self.get_robot_info(robot_name) + if info is not None: + groups.extend(info.get("planning_groups", [])) + return groups + def get_init_joints(self, robot_name: str) -> JointState | None: return getattr(self, "_init_joints", {}).get(robot_name) - def get_planned_path(self, robot_name: str) -> list[JointState] | None: - return getattr(self, "_planned_paths", {}).get(robot_name) - - def get_planned_trajectory_duration(self, robot_name: str) -> float | None: - trajectory = getattr(self, "_planned_trajectories", {}).get(robot_name) - return None if trajectory is None else float(trajectory.duration) - def get_state(self) -> str: state = getattr(self, "_state", "IDLE") return str(getattr(state, "name", state)) @@ -410,34 +474,100 @@ def get_state(self) -> str: def get_error(self) -> str: return str(getattr(self, "_error_message", "")) - def evaluate_joint_target(self, joints: JointState | None, robot_name: str) -> TargetEvaluation: + def get_current_joint_state(self, robot_name: str) -> JointState | None: robot_id = self.robot_id_for_name(robot_name) - if robot_id is None or joints is None: - return {"success": False, "status": "NO_ROBOT", "joint_state": None} world_monitor = getattr(self, "_world_monitor", None) - if world_monitor is None: - return {"success": False, "status": "UNAVAILABLE", "joint_state": None} - collision_free = world_monitor.is_state_valid(robot_id, joints) - return { - "success": True, - "status": "FEASIBLE" if collision_free else "COLLISION", - "message": "Target is collision-free" if collision_free else "Target is in collision", - "collision_free": collision_free, - "ee_pose": world_monitor.get_ee_pose(robot_id, joints), - "joint_state": joints, - } + if robot_id is None or world_monitor is None: + return None + return world_monitor.get_current_joint_state(robot_id) - def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluation: - return { - "success": False, - "joint_state": None, - "status": "UNAVAILABLE", - "message": "No fake pose IK", - "collision_free": False, - } + def check_collision( + self, target_joints: JointState, max_age: float = 1.0 + ) -> CollisionCheckResult: + del max_age + world_monitor = getattr(self, "_world_monitor", None) + if world_monitor is not None and hasattr(world_monitor, "check_collision"): + return world_monitor.check_collision(target_joints) + collision_free = True + if world_monitor is not None: + for robot_name in self.list_robots(): + robot_id = self.robot_id_for_name(robot_name) + if robot_id is not None: + collision_free = collision_free and world_monitor.is_state_valid( + robot_id, target_joints + ) + return CollisionCheckResult( + status="VALID" if collision_free else "COLLISION", + collision_free=collision_free, + message="Target is collision-free" if collision_free else "Target is in collision", + ) + + def forward_kinematics( + self, + group_id: str, + target_joints: JointState | None = None, + max_age: float = 1.0, + ) -> ForwardKinematicsResult: + del max_age + robot_name = group_id.split(":", 1)[0].split("/", 1)[0] + robot_id = self.robot_id_for_name(robot_name) + world_monitor = getattr(self, "_world_monitor", None) + pose = None + if world_monitor is not None and robot_id is not None: + if hasattr(world_monitor, "get_group_ee_pose"): + pose = world_monitor.get_group_ee_pose(group_id, target_joints) + else: + pose = world_monitor.get_ee_pose(robot_id, target_joints) + return ForwardKinematicsResult( + status="VALID", pose=pose, message="Forward kinematics solved" + ) + + def _current_values_by_name(self, robot_name: str) -> dict[str, float]: + current = self.get_current_joint_state(robot_name) + if current is None: + return {} + values: dict[str, float] = {} + for name, position in zip(current.name, current.position, strict=False): + name_str = str(name) + values[name_str] = float(position) + if "/" in name_str: + values[name_str.rsplit("/", 1)[1]] = float(position) + else: + values[f"{robot_name}/{name_str}"] = float(position) + return values + + def inverse_kinematics( + self, + pose_targets: dict[str, PoseStamped], + auxiliary_group_ids: Sequence[str] = (), + seed: JointState | None = None, + ) -> IKResult: + del seed + group_ids = tuple(pose_targets) + tuple(auxiliary_group_ids) + names: list[str] = [] + positions: list[float] = [] + for group_id in group_ids or (DEFAULT_GROUP_ID,): + robot_name = group_id.split(":", 1)[0].split("/", 1)[0] + config = self.get_robot_config(robot_name) + if config is None: + continue + for joint_name in config.joint_names: + names.append(f"{robot_name}/{joint_name}") + positions.append(0.1 + 0.1 * len(positions)) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState({"name": names, "position": positions}), + message="Target is collision-free", + ) + + def plan_to_joint_targets(self, _joint_targets: dict[str, JointState]) -> bool: + return True + + def reset(self) -> bool: + return True -def make_adapter_with_robot() -> InProcessViserAdapter: +def make_module_with_robot() -> tuple[SimpleNamespace, FakeManipulationModule]: current = FakeJointState(["j1", "j2"], position=[0.3, 0.4]) config = make_robot_config( name="arm", @@ -455,16 +585,15 @@ def make_adapter_with_robot() -> InProcessViserAdapter: module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[0.1, 0.2])}, - _planned_paths={}, - _planned_trajectories={}, _state=NamedState(name="IDLE"), _error_message="", _world_monitor=world_monitor, ) - return InProcessViserAdapter( - world_monitor=world_monitor, - manipulation_module=module, - ) + return world_monitor, module + + +def joints_from_values(joint_names: Sequence[str], values: Sequence[float]) -> JointState: + return JointState({"name": list(joint_names), "position": [float(value) for value in values]}) @pytest.fixture @@ -474,12 +603,16 @@ def make_panel() -> Iterator[Callable[..., ViserPanelGui]]: def _make( server: FakeGuiServer | FakeServer, - adapter: InProcessViserAdapter, + module_context: tuple[SimpleNamespace, FakeManipulationModule], config: ViserVisualizationConfig | None = None, scene: ViserManipulationScene | None = None, ) -> ViserPanelGui: gui = ViserPanelGui( - server, adapter, config or ViserVisualizationConfig(panel_enabled=True), scene + server, + module_context[0], + module_context[1], + config or ViserVisualizationConfig(panel_enabled=True), + scene, ) gui.start() panels.append(gui) @@ -498,14 +631,50 @@ def test_gui_builds_controls_in_manipulation_panel_folder( make_panel: Callable[..., ViserPanelGui], ) -> None: server = FakeGuiServer() - adapter = make_adapter_with_robot() - gui = make_panel(server, adapter, ViserVisualizationConfig()) + module_context = make_module_with_robot() + gui = make_panel(server, module_context, ViserVisualizationConfig()) assert server.folders assert server.folders[0].label == "Manipulation Panel" assert server.folders[0].kwargs == {"expand_by_default": True} assert "status" in gui._handles - assert "robot" in gui._handles + assert "robot" not in gui._handles + assert "planning_groups_heading" in gui._handles + assert "target_heading" in gui._handles + assert "target_summary" in gui._handles + assert "actions_heading" in gui._handles assert "plan" in gui._handles + assert "select_all_manipulators" not in gui._handles + assert "clear_group_selection" not in gui._handles + assert "plan_controls_heading" in gui._handles + assert "actions_folder" not in gui._handles + assert "joint_control_folder" in gui._handles + handle_order = list(gui._handles) + assert handle_order.index(f"group:{DEFAULT_GROUP_ID}") < handle_order.index("plan") + assert handle_order.index("target_summary") < handle_order.index("plan") + assert handle_order.index("plan") < handle_order.index("plan_controls_heading") + assert handle_order.index("plan_controls_heading") < handle_order.index("preview") + assert handle_order.index("preview") < handle_order.index("execute") + assert handle_order.index("clear") < handle_order.index("joint_control_folder") + assert isinstance(gui._handles["status"], GuiMarkdownHandle) + assert "Starting" not in gui._handles["status"].value + assert isinstance(gui._handles["target_summary"], GuiMarkdownHandle) + assert "Feasibility:" in gui._handles["target_summary"].value + assert "Primary:" not in gui._handles["target_summary"].value + assert "Auxiliary:" not in gui._handles["target_summary"].value + assert "Ghosts:" not in gui._handles["target_summary"].value + assert isinstance(gui._handles["plan_controls_heading"], GuiMarkdownHandle) + assert "Plan controls" in gui._handles["plan_controls_heading"].value + plan_button = gui._handles["plan"] + assert isinstance(plan_button, GuiButtonHandle) + assert plan_button.color == PRIMARY_ACTION_COLOR + group_button = gui._handles[f"group:{DEFAULT_GROUP_ID}"] + assert isinstance(group_button, GuiButtonHandle) + assert group_button.label == "arm" + assert group_button.color == ACTIVE_GROUP_COLOR + joint_folder = gui._handles["joint_control_folder"] + assert isinstance(joint_folder, FakeFolder) + assert joint_folder.label == "Joint Control" + assert joint_folder.kwargs == {"expand_by_default": False} assert gui._operation_worker._timeout_seconds is None @@ -517,8 +686,8 @@ def test_gui_scene_grid_checkbox_toggles_reference_grid( grid_server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 ) server = FakeGuiServer() - adapter = make_adapter_with_robot() - make_panel(server, adapter, ViserVisualizationConfig(), scene) + module_context = make_module_with_robot() + make_panel(server, module_context, ViserVisualizationConfig(), scene) assert grid_server.grids assert server.checkboxes["Scene grid"].value is True server.checkboxes["Scene grid"].update_callback( @@ -539,16 +708,13 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( scene = ViserManipulationScene( grid_server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 ) - adapter = make_adapter_with_robot() - gui = make_panel(server, adapter, ViserVisualizationConfig(), scene) - robot_dropdown = gui._handles["robot"] + module_context = make_module_with_robot() + gui = make_panel(server, module_context, ViserVisualizationConfig(), scene) plan_button = server.buttons["Plan"] grid = grid_server.grids[0] handles = list(gui._handles.values()) gui.close() - if isinstance(robot_dropdown, GuiDropdownHandle) and robot_dropdown.update_callback is not None: - robot_dropdown.update_callback(SimpleNamespace(target=SimpleNamespace(value="arm"))) if plan_button.click_callback is not None: plan_button.click_callback(SimpleNamespace()) gui._set_scene_grid_visible(False) @@ -561,15 +727,15 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( def test_gui_ignores_target_evaluation_after_close( make_panel: Callable[..., ViserPanelGui], ) -> None: - adapter = make_adapter_with_robot() - gui = make_panel(FakeGuiServer(), adapter) + module_context = make_module_with_robot() + gui = make_panel(FakeGuiServer(), module_context) gui.state.selected_robot = "arm" sequence_id = gui.state.next_sequence_id() request = TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name="arm", - joints=FakeJointState(["j1", "j2"], position=[0.1, 0.2]), + group_ids=(DEFAULT_GROUP_ID,), + joint_targets={DEFAULT_GROUP_ID: FakeJointState(["arm/j1", "arm/j2"], position=[0.1, 0.2])}, ) gui.close() @@ -592,12 +758,12 @@ def test_dimos_theme_configures_supported_viser_chrome() -> None: assert apply_dimos_theme(server) is True assert server.theme_kwargs is not None - assert server.theme_kwargs["brand_color"] == (22, 130, 163) + assert server.theme_kwargs["brand_color"] == (0, 153, 255) assert server.theme_kwargs["dark_mode"] is True assert server.theme_kwargs["show_logo"] is False assert server.theme_kwargs["show_share_button"] is False - assert server.theme_kwargs["control_layout"] == "collapsible" - assert server.theme_kwargs["control_width"] == "medium" + assert server.theme_kwargs["control_layout"] == "fixed" + assert server.theme_kwargs["control_width"] == "large" def test_dimos_theme_configures_titlebar_when_supported(monkeypatch: pytest.MonkeyPatch) -> None: @@ -737,8 +903,10 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles scene.register_robot("robot1", config) target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.show_preview("robot1") assert all(mesh.visible is True for mesh in preview._meshes) assert all(mesh.visible is True for mesh in target._meshes) @@ -750,7 +918,7 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles assert all(mesh.visible is False for mesh in preview._meshes) -def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> None: +def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: server = FakeServer() urdfs = [FakeViserUrdfWithMeshes(("joint1",)) for _ in range(3)] scene = ViserManipulationScene(server, lambda *args, **kwargs: urdfs.pop(0), preview_fps=10.0) @@ -768,12 +936,16 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.25])) assert current.cfg == [0.25] assert target.cfg == [0.25] assert preview.cfg is None + assert all(mesh.visible is False for mesh in target._meshes) + + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.set_target_joints("robot1", ["joint1"], [0.8]) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.1])) @@ -781,6 +953,51 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N assert target.cfg == [0.8] assert preview.cfg is None + scene.set_target_active("robot1", False) + assert all(mesh.visible is False for mesh in target._meshes) + + +def test_scene_parents_urdfs_under_base_pose_frame() -> None: + server = FakeServer() + root_node_names: list[str] = [] + + def make_urdf(*_: object, **kwargs: object) -> FakeViserUrdfWithMeshes: + root_node_names.append(str(kwargs["root_node_name"])) + return FakeViserUrdfWithMeshes(("joint1",)) + + scene = ViserManipulationScene(server, make_urdf, preview_fps=10.0) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + base_pose=PoseStamped( + position=Vector3(1.0, 2.0, 3.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + ) + + scene.register_robot("robot1", config) + + assert [frame.name for frame in server.frames] == [ + "/robots/robot1/current/base_pose", + "/targets/robot1/target/base_pose", + "/previews/robot1/ghost/base_pose", + ] + assert [frame.kwargs["position"] for frame in server.frames] == [ + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + ] + assert root_node_names == [ + "/robots/robot1/current/base_pose/urdf", + "/targets/robot1/target/base_pose/urdf", + "/previews/robot1/ghost/base_pose/urdf", + ] + def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback() -> None: server = FakeServer() @@ -815,7 +1032,74 @@ def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback( assert ok is True assert preview.cfg == [1.0] assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) + + +def test_group_preview_animation_updates_all_tracks_on_shared_frame_clock( + monkeypatch: pytest.MonkeyPatch, +) -> None: + server = FakeServer() + scene = ViserManipulationScene( + server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 + ) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + ) + scene.register_robot("left", config) + scene.register_robot("right", config) + updates: list[tuple[str, tuple[str, ...], tuple[float, ...]]] = [] + sleep_calls: list[float] = [] + + def record_preview_joints( + robot_id: str, joint_names: Sequence[str], joints: Sequence[float] + ) -> None: + updates.append((robot_id, tuple(joint_names), tuple(joints))) + + monkeypatch.setattr(scene, "_set_preview_ghost_joints", record_preview_joints) + monkeypatch.setattr(scene_module.time, "sleep", sleep_calls.append) + + ok = scene.animate_preview( + GroupPreviewAnimation( + group_ids=("left/arm", "right/arm"), + tracks=( + PreviewTrack( + robot_id="left", + group_ids=("left/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[0.0]), + FakeJointState(["joint1"], position=[1.0]), + ), + ), + PreviewTrack( + robot_id="right", + group_ids=("right/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[10.0]), + FakeJointState(["joint1"], position=[11.0]), + ), + ), + ), + ), + duration=0.0, + ) + + assert ok is True + assert updates == [ + ("left", ("joint1",), (0.0,)), + ("right", ("joint1",), (10.0,)), + ("left", ("joint1",), (1.0,)), + ("right", ("joint1",), (11.0,)), + ] + assert sleep_calls == [0.0] + assert scene._preview_visible == {"left": False, "right": False} def test_scene_target_helpers_handle_missing_robot_and_pose() -> None: @@ -899,105 +1183,6 @@ def test_joint_path_frame_edge_cases_and_empty_animation() -> None: assert sleep_calls == [] -def test_adapter_copies_joint_state_and_delegates_to_module() -> None: - copied = FakeJointState(["j1"], position=[1.0], velocity=[2.0], effort=[3.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", SimpleNamespace(), None)}, - _planned_paths={"arm": [copied]}, - _planned_trajectories={}, - plan_to_pose=lambda pose, robot_name=None: (pose, robot_name), - plan_to_joints=lambda joints, robot_name=None: (joints, robot_name), - preview_path=lambda robot_name=None: robot_name, - execute=lambda robot_name=None: robot_name, - cancel=lambda: True, - clear_planned_path=lambda: True, - ) - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: copied, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=lambda robot_id, joint_state: True, - get_ee_pose=lambda robot_id, joint_state=None: (robot_id, joint_state), - ) - module._world_monitor = world_monitor - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) - - planned = adapter.get_planned_path("arm") - assert planned is not None - assert planned[0] is not copied - assert planned[0].name is not copied.name - assert planned[0].position is not copied.position - - current = adapter.get_current_joint_state("arm") - assert current is not copied - assert current.name is not copied.name - - assert adapter.plan_to_pose("pose", "arm") == ("pose", "arm") - assert adapter.preview_path("arm") == "arm" - assert adapter.evaluate_joint_target(planned[0], "arm")["status"] == "FEASIBLE" - - -def test_adapter_evaluate_joint_target_uses_world_monitor_and_copies_input() -> None: - original = FakeJointState(["arm/j1", "j2"], position=[1.0, 2.0]) - seen = {} - - def is_state_valid(robot_id, joint_state) -> bool: - seen["robot_id"] = robot_id - seen["joint_state"] = joint_state - return True - - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: None, - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=is_state_valid, - get_ee_pose=lambda robot_id, joint_state=None: (robot_id, joint_state), - ) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", SimpleNamespace(), None)}, - _world_monitor=world_monitor, - ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) - - result = adapter.evaluate_joint_target(original, "arm") - - assert result["success"] is True - assert result["status"] == "FEASIBLE" - assert seen["robot_id"] == "robot-1" - assert seen["joint_state"] is not original - assert seen["joint_state"].name == ["arm/j1", "j2"] - assert seen["joint_state"].position == [1.0, 2.0] - - -def test_obstacle_collision_marks_joint_target_infeasible() -> None: - obstacle = SimpleNamespace(name="blocking_box", blocked_joint_min=0.5) - - def is_state_valid(robot_id, joint_state) -> bool: - return bool(joint_state.position[0] < obstacle.blocked_joint_min) - - world_monitor = SimpleNamespace( - get_current_joint_state=lambda robot_id: FakeJointState(["j1"], position=[0.0]), - is_state_stale=lambda robot_id, max_age=1.0: False, - is_state_valid=is_state_valid, - get_ee_pose=lambda robot_id, joint_state=None: SimpleNamespace( - position=SimpleNamespace(x=0.0, y=0.0, z=0.0) - ), - ) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", SimpleNamespace(joint_names=["j1"]), None)}, - _world_monitor=world_monitor, - ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) - - free = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[0.25]), "arm") - colliding = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[0.75]), "arm") - - assert free["success"] is True - assert free["status"] == "FEASIBLE" - assert free["collision_free"] is True - assert colliding["success"] is True - assert colliding["status"] == "COLLISION" - assert colliding["collision_free"] is False - - def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: server = FakeServer() scene = ViserManipulationScene( @@ -1028,7 +1213,7 @@ def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: assert all(mesh.visible is True for mesh in preview._meshes) scene.hide_preview("robot1") assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) def test_scene_transform_controls_update_pose_callback_and_visual_state() -> None: @@ -1112,25 +1297,153 @@ def test_gui_initializes_pose_selector_to_current_ee_pose( orientation=SimpleNamespace(w=0.9, x=0.1, y=0.2, z=0.3), ) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, get_ee_pose=lambda robot_id, joint_state=None: current_pose, ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) scene = ViserManipulationScene( FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 ) - gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - control = scene._handles["robot-1:ee_control"] + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] assert control.position == (0.1, 0.2, 0.3) assert control.wxyz == (0.9, 0.1, 0.2, 0.3) assert gui.state.cartesian_target is current_pose +def test_gui_removes_pose_selector_when_group_is_deselected( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1"], position=[0.25]) + current_pose = SimpleNamespace( + position=SimpleNamespace(x=0.1, y=0.2, z=0.3), + orientation=SimpleNamespace(w=1.0, x=0.0, y=0.0, z=0.0), + ) + config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + get_ee_pose=lambda robot_id, joint_state=None: current_pose, + ) + module_context = (world_monitor, module) + scene = ViserManipulationScene( + FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 + ) + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] + + gui._set_group_selected(DEFAULT_GROUP_ID, False) + + assert f"{DEFAULT_GROUP_ID}:ee_control" not in scene._handles + assert control.removed is True + + +def test_gui_group_selector_derives_primary_and_auxiliary_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "grip"], position=[0.25, 0.5]) + config = make_robot_config(joint_names=["j1", "grip"], home_joints=[0.0, 0.0]) + pose_group = make_planning_group_info("arm", config) + auxiliary_group = make_planning_group_info( + "arm", config, group_name="gripper", has_pose_target=False + ) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, + _planning_groups=[pose_group, auxiliary_group], + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + module_context = (world_monitor, module) + target_controls = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: target_controls.append(args) or object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + server = FakeGuiServer() + + gui = make_panel(server, module_context, ViserVisualizationConfig(panel_enabled=True), scene) + assert "robot" not in gui._handles + pose_button = gui._handles["group:arm:manipulator"] + aux_button = gui._handles["group:arm:gripper"] + assert isinstance(pose_button, GuiButtonHandle) + assert isinstance(aux_button, GuiButtonHandle) + assert pose_button.label == "arm" + assert pose_button.color == ACTIVE_GROUP_COLOR + assert aux_button.label == "arm gripper" + assert aux_button.color == INACTIVE_GROUP_COLOR + + assert aux_button.click_callback is not None + aux_button.click_callback(SimpleNamespace(target=aux_button)) + + assert gui.state.selected_group_ids == ("arm:manipulator", "arm:gripper") + assert gui.state.auxiliary_group_ids == ("arm:gripper",) + assert aux_button.label == "arm gripper" + assert aux_button.color == ACTIVE_GROUP_COLOR + assert [call[0] for call in target_controls] == ["arm:manipulator"] + + +def test_gui_target_ghost_visibility_follows_active_selected_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + left_config = make_robot_config(name="left", joint_names=["j1"], home_joints=[0.0]) + right_config = make_robot_config(name="right", joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule( + _robots={ + "left": ("left-id", left_config, None), + "right": ("right-id", right_config, None), + } + ) + current = FakeJointState(["j1"], position=[0.0]) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + module_context = (world_monitor, module) + active_updates = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: active_updates.append(args), + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + + assert active_updates[-2:] == [("left-id", True), ("right-id", False)] + gui._set_group_selected("right:manipulator", True) + assert active_updates[-2:] == [("left-id", True), ("right-id", True)] + gui._set_group_selected("left:manipulator", False) + assert active_updates[-2:] == [("left-id", False), ("right-id", True)] + + def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callbacks( make_panel: Callable[..., ViserPanelGui], ) -> None: @@ -1139,8 +1452,6 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[-1.0, -2.0])}, - _planned_paths={}, - _planned_trajectories={}, ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, @@ -1148,14 +1459,14 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) - gui = make_panel(FakeGuiServer(), adapter) + module_context = (world_monitor, module) + gui = make_panel(FakeGuiServer(), module_context) assert gui._handles["preset"].options == ["Select preset...", "Init", "Current", "Home"] - assert list(gui._joint_sliders) == ["j1", "j2"] + assert list(gui._joint_sliders) == ["arm/j1", "arm/j2"] gui._apply_preset("Home") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] gui._apply_preset("Current") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.5] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.5] gui._submit_execute() assert gui.state.error == "Panel execution disabled; set allow_plan_execute=True to enable" @@ -1165,42 +1476,37 @@ def test_gui_rebuilding_joint_sliders_removes_stale_viser_handles( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[1.0, 2.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) server = FakeGuiServer() - gui = make_panel(server, adapter) + gui = make_panel(server, module_context) stale_sliders = list(server.sliders) assert [slider.value for slider in stale_sliders] == [0.0, 0.0] current.position = [-0.738, -0.2826151825863572] + gui.state.target_joints = None + gui.state.group_joint_targets.clear() gui._build_joint_sliders() assert all(slider.removed is True for slider in stale_sliders) - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [ + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [ -0.738, -0.2826151825863572, ] def test_gui_parses_numpy_transform_control_arrays() -> None: - gui = ViserPanelGui(FakeGuiServer(), make_adapter_with_robot(), ViserVisualizationConfig()) - - pose = gui._pose_from_transform_target( - SimpleNamespace( - position=np.array([1.0, 2.0, 3.0]), - wxyz=np.array([0.5, 0.1, 0.2, 0.3]), - ) + pose = pose_from_transform_values( + np.array([1.0, 2.0, 3.0]), + np.array([0.5, 0.1, 0.2, 0.3]), ) - assert pose is not None assert list(pose.position) == [1.0, 2.0, 3.0] assert list(pose.orientation) == [0.1, 0.2, 0.3, 0.5] @@ -1212,8 +1518,6 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( config = make_robot_config(joint_names=["j1"], home_joints=[0.5]) module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, - _planned_paths={}, - _planned_trajectories={}, execute=lambda robot_name=None: False, ) world_monitor = SimpleNamespace( @@ -1222,16 +1526,13 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - adapter = InProcessViserAdapter( - world_monitor=world_monitor, - manipulation_module=module, - ) - gui = make_panel(FakeGuiServer(), adapter) + module_context = (world_monitor, module) + gui = make_panel(FakeGuiServer(), module_context) gui.refresh() assert gui.state.selected_robot == "arm" - assert list(gui._joint_sliders) == ["j1"] + assert list(gui._joint_sliders) == ["arm/j1"] gui._apply_preset("Home") - assert gui._joint_sliders["j1"].value == 0.5 + assert gui._joint_sliders["arm/j1"].value == 0.5 gui._submit_execute() assert "Panel execution disabled" in gui.state.error @@ -1243,16 +1544,14 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) target_pose = SimpleNamespace(position=SimpleNamespace(x=0.2, y=0.3, z=0.4)) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: target_pose, ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) target_updates = [] target_pose_updates = [] scene = SimpleNamespace( @@ -1262,45 +1561,56 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) - gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) requests = [] gui._worker.stop() gui._worker = SimpleNamespace( submit=lambda request: requests.append(request), stop=lambda: None ) - gui._joint_sliders["j1"].value = 0.25 - gui._joint_sliders["j2"].value = 0.75 + gui._joint_sliders["arm/j1"].value = 0.25 + gui._joint_sliders["arm/j2"].value = 0.75 gui._submit_joint_target_evaluation() assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) - assert target_pose_updates[-1] == ("robot-1", target_pose) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, target_pose) assert requests[-1].source == "joints" - stale_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - fresh_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + stale_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + fresh_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) gui.state.latest_sequence_id = 2 gui._apply_target_evaluation_result( stale_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [9.0, 9.0]), + "target_joints": joints_from_values(["arm/j1", "arm/j2"], [9.0, 9.0]), }, ) - assert gui.state.joint_target == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [0.25, 0.75] + joint_bar_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) gui._apply_target_evaluation_result( fresh_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: joint_bar_pose}, }, ) assert gui.state.target_status == TargetStatus.FEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.FEASIBLE - assert gui.state.joint_target == [1.0, 2.0] - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.75] assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, joint_bar_pose) def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( @@ -1308,30 +1618,35 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, is_state_valid=lambda robot_id, joint_state: True, get_ee_pose=lambda robot_id, joint_state=None: None, ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) target_joint_updates = [] target_pose_updates = [] scene = SimpleNamespace( has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: None, + ensure_target_controls=lambda *args: object(), set_target_joints=lambda *args: target_joint_updates.append(args) or True, set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) - gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - gui.state.cartesian_target = Pose( - {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + gui._handles[f"ee_control:{DEFAULT_GROUP_ID}"] = object() + dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) + solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) + gui.state.cartesian_target = dragged_pose + gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose + target_pose_updates.clear() + request = TargetEvaluationRequest( + sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) ) - request = TargetEvaluationRequest(sequence_id=1, source="cartesian", robot_name="arm") gui.state.latest_sequence_id = 1 gui._apply_target_evaluation_result( @@ -1339,14 +1654,128 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: solved_pose}, }, ) assert gui.state.target_status == TargetStatus.FEASIBLE - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) assert target_pose_updates == [] + assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose + assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose + + +def test_gui_cartesian_collision_still_updates_target_ghost_red( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) + config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: False, + get_ee_pose=lambda robot_id, joint_state=None: None, + ) + module_context = (world_monitor, module) + target_joint_updates = [] + target_pose_updates = [] + visual_states = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: object(), + set_target_joints=lambda *args: target_joint_updates.append(args) or True, + set_target_pose=lambda *args: target_pose_updates.append(args), + set_target_visual_state=lambda *args: visual_states.append(args), + ) + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) + solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) + gui.state.cartesian_target = dragged_pose + gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose + target_joint_updates.clear() + target_pose_updates.clear() + visual_states.clear() + request = TargetEvaluationRequest( + sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) + ) + gui.state.latest_sequence_id = 1 + + gui._apply_target_evaluation_result( + request, + { + "success": False, + "status": "COLLISION", + "message": "Target is in collision", + "collision_free": False, + "target_joints": joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: solved_pose}, + }, + ) + + assert gui.state.target_status == TargetStatus.INFEASIBLE + assert gui.state.feasibility.status == FeasibilityStatus.COLLISION + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [1.0, 2.0] + assert gui.state.last_valid_target_joints is None + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] + assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) + assert (DEFAULT_GROUP_ID, False) in visual_states + assert ("robot-1", False) in visual_states + assert target_pose_updates == [] + assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose + assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose + + +def test_gui_can_disable_collision_check_for_cartesian_target_evaluation( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) + config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.0, 0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: False, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + module_context = (world_monitor, module) + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + gui = make_panel( + FakeGuiServer(), + module_context, + ViserVisualizationConfig(panel_enabled=True), + scene, + ) + request = TargetEvaluationRequest( + sequence_id=1, + source="cartesian", + group_ids=(DEFAULT_GROUP_ID,), + pose_targets={ + DEFAULT_GROUP_ID: Pose( + {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} + ) + }, + check_collision=False, + ) + + result = gui._handle_target_evaluation_request(request) + + assert result["success"] is True + assert result["collision_free"] is True + assert result["message"] == "Target collision check skipped" def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( @@ -1354,9 +1783,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( ) -> None: current = FakeJointState(["j1"], position=[0.0]) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1366,7 +1793,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( ), ) module._world_monitor = world_monitor - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) visual_states = [] scene = SimpleNamespace( has_reference_grid=lambda: False, @@ -1375,10 +1802,14 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( set_target_pose=lambda *args: None, set_target_visual_state=lambda *args: visual_states.append(args), ) - gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") + gui = make_panel( + FakeGuiServer(), module_context, ViserVisualizationConfig(panel_enabled=True), scene + ) + request = TargetEvaluationRequest(sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,)) gui.state.latest_sequence_id = 1 - result = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[1.0]), "arm") + result = gui._evaluate_joint_target_set( + {DEFAULT_GROUP_ID: FakeJointState(["arm/j1"], position=[1.0])} + ) gui._apply_target_evaluation_result(request, result) @@ -1386,7 +1817,8 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( assert gui.state.target_status == TargetStatus.INFEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.COLLISION assert gui.state.error == "Target is in collision" - assert visual_states[-1] == ("robot-1", False) + assert (DEFAULT_GROUP_ID, False) in visual_states + assert ("robot-1", False) in visual_states def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( @@ -1400,8 +1832,6 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( _robots={ "arm": ("robot-1", make_robot_config(joint_names=["j1"], home_joints=[1.0]), None) }, - _planned_paths={"arm": planned}, - _planned_trajectories={}, _state=NamedState(name="IDLE"), execute=lambda robot_name=None: executed.append(robot_name) or True, clear_planned_path=lambda: cleared.append(True) or True, @@ -1414,10 +1844,10 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( position=SimpleNamespace(x=0.0, y=0.0, z=0.0) ), ) - adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + module_context = (world_monitor, module) gui = make_panel( FakeGuiServer(), - adapter, + module_context, ViserVisualizationConfig( panel_enabled=True, allow_plan_execute=True, current_match_tolerance=0.05 ), @@ -1433,19 +1863,20 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.target_status = TargetStatus.FEASIBLE gui.state.plan_state = PanelPlanState( status=PlanStatus.FRESH, - robot="arm", - start_joints_snapshot=[1.2], + group_ids=(DEFAULT_GROUP_ID,), + start_joints_snapshot={DEFAULT_GROUP_ID: [1.2]}, planned_path=planned, ) + gui.state.target_joints = FakeJointState(["arm/j1"], position=[2.0]) gui._submit_execute() assert executed == [] assert "Cannot execute" in gui.state.error gui.state.action_status = ActionStatus.IDLE gui.state.error = "" - gui.state.plan_state.start_joints_snapshot = [1.0] + gui.state.plan_state.start_joints_snapshot = {DEFAULT_GROUP_ID: [1.0]} gui._submit_execute() - assert executed == ["arm"] + assert executed == [None] gui._submit_clear() assert cleared == [True] @@ -1456,8 +1887,8 @@ def test_gui_plan_target_failure_recovers_action_state( make_panel: Callable[..., ViserPanelGui], monkeypatch: pytest.MonkeyPatch, ) -> None: - adapter = make_adapter_with_robot() - gui = make_panel(FakeGuiServer(), adapter) + module_context = make_module_with_robot() + gui = make_panel(FakeGuiServer(), module_context) gui._operation_worker.stop() monkeypatch.setattr( gui, @@ -1466,7 +1897,8 @@ def test_gui_plan_target_failure_recovers_action_state( submit=lambda operation, **_kwargs: operation(), stop=lambda timeout=2.0: None ), ) - gui.state.selected_robot = "missing" + gui.state.selected_group_ids = ("missing",) + gui.state.target_joints = JointState({"name": ["missing/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -1474,7 +1906,7 @@ def test_gui_plan_target_failure_recovers_action_state( assert gui.state.action_status == ActionStatus.IDLE assert gui.state.plan_state.status == PlanStatus.FAILED - assert gui.state.error == "No robot config" + assert gui.state.error == "Unknown planning group: missing" assert gui.state.last_result == "plan_to_joints=False" @@ -1483,8 +1915,8 @@ def test_gui_resets_fault_before_replanning( monkeypatch: pytest.MonkeyPatch, ) -> None: calls = [] - adapter = make_adapter_with_robot() - gui = make_panel(FakeGuiServer(), adapter) + module_context = make_module_with_robot() + gui = make_panel(FakeGuiServer(), module_context) gui._operation_worker.stop() monkeypatch.setattr( gui, @@ -1498,12 +1930,12 @@ def reset() -> bool: calls.append("reset") return True - def plan_to_joints(_joints: JointState, _robot_name: str | None = None) -> bool: + def plan_target_set(_joint_targets: dict[str, JointState]) -> bool: calls.append("plan") return True - monkeypatch.setattr(adapter, "reset", reset) - monkeypatch.setattr(adapter, "plan_to_joints", plan_to_joints) + monkeypatch.setattr(module_context[1], "reset", reset) + monkeypatch.setattr(module_context[1], "plan_to_joint_targets", plan_target_set) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "FAULT" @@ -1562,8 +1994,12 @@ def operation() -> None: def test_target_evaluation_worker_coalesces_pending_requests() -> None: worker = TargetEvaluationWorker(lambda request: {}, lambda request, result: None) - old_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - new_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + old_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + new_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) worker.submit(old_request) worker.submit(new_request) diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 19f1f2da60..36977ce313 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -22,12 +22,13 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.models import PlanningSceneInfo +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningSceneInfo from dimos.manipulation.visualization.viser import ( runtime as runtime_module, visualizer as visualizer_module, ) -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import GroupPreviewAnimation from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import ViserRuntime from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer @@ -62,7 +63,7 @@ def fake_robot_config(name: str) -> RobotModelConfig: name=name, model_path=Path(f"{name}.urdf"), base_pose=PoseStamped(), - joint_names=[], + joint_names=["joint1"], end_effector_link="ee_link", ) @@ -121,10 +122,12 @@ class FakeGui: def __init__( self, server: FakeServer, - adapter: InProcessViserAdapter, + world_monitor: object, + manipulation_module: object, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: + del world_monitor, manipulation_module, config, scene calls.append(("create", "gui")) def start(self) -> None: @@ -199,10 +202,12 @@ class FakeGui: def __init__( self, server: FakeServer, - adapter: InProcessViserAdapter, + world_monitor: object, + manipulation_module: object, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: + del world_monitor, manipulation_module, config, scene pass def start(self) -> None: @@ -305,10 +310,12 @@ class FailingGui: def __init__( self, server: FakeServer, - adapter: InProcessViserAdapter, + world_monitor: object, + manipulation_module: object, config: ViserVisualizationConfig, scene: FakeScene, ) -> None: + del world_monitor, manipulation_module, config, scene pass def start(self) -> None: @@ -407,18 +414,30 @@ def show_preview(self, robot_id: str) -> None: def hide_preview(self, robot_id: str) -> None: calls.append(("hide", robot_id)) - def animate_path(self, robot_id: str, path: list[JointState], duration: float) -> None: - assert path == [current] + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert preview.group_ids == ("arm/manipulator",) + assert len(preview.tracks) == 1 + assert preview.tracks[0].path == (current,) assert duration == 1.5 - calls.append(("animate", robot_id)) + calls.append(("animate", preview.tracks[0].robot_id)) def close(self) -> None: calls.append(("scene", "close")) - world_monitor = SimpleNamespace(get_current_joint_state=lambda _robot_id: current) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda _robot_id: current, + planning_groups=SimpleNamespace( + select=lambda _group_ids: SimpleNamespace( + groups=(SimpleNamespace(id="arm/manipulator", robot_name="arm"),), + robot_names=("arm",), + ) + ), + ) + robot_config = fake_robot_config("arm") manipulation_module = SimpleNamespace( - robot_items=lambda: [("arm", "robot-1", fake_robot_config("arm"))], + robot_items=lambda: [("arm", "robot-1", robot_config)], robot_id_for_name=lambda robot_name: "robot-1" if robot_name == "arm" else None, + get_robot_config=lambda robot_name: robot_config if robot_name == "arm" else None, ) monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) @@ -430,9 +449,14 @@ def close(self) -> None: ) visualizer.publish_visualization() - visualizer.show_preview("robot-1") - visualizer.hide_preview("robot-1") - visualizer.animate_path("robot-1", [current], duration=1.5) + visualizer.show_preview(("arm/manipulator",)) + visualizer.hide_preview(("arm/manipulator",)) + plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/joint1"], position=[0.5])], + status=PlanningStatus.SUCCESS, + ) + visualizer.animate_plan(plan, duration=1.5) visualizer.close() visualizer.publish_visualization() @@ -446,3 +470,86 @@ def close(self) -> None: ("scene", "close"), ("runtime", "close"), ] + + +def test_visualizer_animates_multi_robot_plan_as_one_group_preview( + monkeypatch: pytest.MonkeyPatch, +) -> None: + previews: list[GroupPreviewAnimation] = [] + + class FakeRuntime: + url = "http://localhost:8095" + + def __init__(self, config: ViserVisualizationConfig) -> None: + self.config = config + + def start(self) -> FakeServer: + return FakeServer() + + def close(self) -> None: + pass + + class FakeScene: + def __init__( + self, + server: FakeServer, + viser_urdf: type[FakeViserUrdf], + *, + preview_fps: float, + ) -> None: + pass + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert duration == 2.0 + previews.append(preview) + + def close(self) -> None: + pass + + groups = ( + SimpleNamespace(id="left/arm", robot_name="left"), + SimpleNamespace(id="right/arm", robot_name="right"), + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: JointState( + {"name": ["joint1"], "position": [0.0 if robot_id == "left-id" else 10.0]} + ), + planning_groups=SimpleNamespace( + select=lambda _group_ids: SimpleNamespace(groups=groups, robot_names=("left", "right")) + ), + ) + configs = {"left": fake_robot_config("left"), "right": fake_robot_config("right")} + manipulation_module = SimpleNamespace( + robot_id_for_name=lambda robot_name: f"{robot_name}-id" if robot_name in configs else None, + get_robot_config=lambda robot_name: configs.get(robot_name), + ) + monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) + monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) + monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) + visualizer = ViserManipulationVisualizer( + world_monitor=world_monitor, + manipulation_module=manipulation_module, + config=ViserVisualizationConfig(panel_enabled=False), + ) + + visualizer.animate_plan( + GeneratedPlan( + group_ids=("left/arm", "right/arm"), + path=[ + JointState(name=["left/joint1", "right/joint1"], position=[0.0, 10.0]), + JointState(name=["left/joint1", "right/joint1"], position=[1.0, 11.0]), + ], + status=PlanningStatus.SUCCESS, + ), + duration=2.0, + ) + + assert len(previews) == 1 + preview = previews[0] + assert preview.group_ids == ("left/arm", "right/arm") + assert [(track.robot_id, track.group_ids) for track in preview.tracks] == [ + ("left-id", ("left/arm",)), + ("right-id", ("right/arm",)), + ] + assert [tuple(point.position) for point in preview.tracks[0].path] == [(0.0,), (1.0,)] + assert [tuple(point.position) for point in preview.tracks[1].path] == [(10.0,), (11.0,)] diff --git a/dimos/manipulation/visualization/viser/theme.py b/dimos/manipulation/visualization/viser/theme.py index 339be04492..d38767d1da 100644 --- a/dimos/manipulation/visualization/viser/theme.py +++ b/dimos/manipulation/visualization/viser/theme.py @@ -36,7 +36,7 @@ DIMOS_THEME_TITLE = "DimOS Manipulation" DIMOS_THEME_URL = "https://github.com/dimensionalOS/dimos" -DIMOS_BRAND_COLOR = (22, 130, 163) +DIMOS_BRAND_COLOR = (0, 153, 255) DIMOS_LOGO_PATH = Path(__file__).with_name("assets") / "dimensional-logo.svg" logger = setup_logger() @@ -86,8 +86,8 @@ def _configure_theme(server: ViserServer, titlebar_content: TitlebarConfig | Non try: server.gui.configure_theme( titlebar_content=titlebar_content, - control_layout="collapsible", - control_width="medium", + control_layout="fixed", + control_width="large", dark_mode=True, show_logo=False, show_share_button=False, diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 2a44008118..62c7c2cf45 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -19,7 +19,10 @@ from typing import TYPE_CHECKING from dimos.manipulation.planning.groups.identifiers import make_global_joint_name -from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, +) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.runtime import ( @@ -71,7 +74,6 @@ def __init__( self.config = config or ViserVisualizationConfig() self._runtime: ViserRuntime | None = None self._server: ViserServer | None = None - self._adapter: InProcessViserAdapter | None = None self._scene: ViserManipulationScene | None = None self._gui: ViserPanelGui | None = None self._closed = False @@ -90,14 +92,11 @@ def _ensure_started(self) -> None: ViserUrdf, preview_fps=self.config.preview_fps, ) - adapter = InProcessViserAdapter( - world_monitor=self._world_monitor, - manipulation_module=self._manipulation_module, - ) gui = ( ViserPanelGui( server, - adapter, + self._world_monitor, + self._manipulation_module, self.config, scene, ) @@ -117,14 +116,12 @@ def _ensure_started(self) -> None: runtime.close() self._runtime = None self._server = None - self._adapter = None self._scene = None self._gui = None self._closed = True raise self._runtime = runtime self._server = server - self._adapter = adapter self._scene = scene self._gui = gui self._closed = False @@ -154,10 +151,17 @@ def publish_visualization(self, ctx: None = None) -> None: if self._closed: return self._ensure_started() - if self._adapter is None or self._scene is None: + if self._scene is None: return - for robot_name, robot_id, _config in self._adapter.robot_items(): - current = self._adapter.get_current_joint_state(robot_name) + for robot_name, robot_id, _config in self._manipulation_module.robot_items(): + get_current_joint_state = getattr( + self._manipulation_module, "get_current_joint_state", None + ) + current = ( + get_current_joint_state(robot_name) + if callable(get_current_joint_state) + else self._world_monitor.get_current_joint_state(robot_id) + ) self._scene.update_current_robot(str(robot_id), current) if self._gui is not None: self._gui.refresh() @@ -178,50 +182,57 @@ def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: for robot_id in self._robot_ids_for_groups(group_ids): self._scene.hide_preview(str(robot_id)) - def animate_path( - self, - robot_id: str, - path: list[JointState], - duration: float = 3.0, - ) -> None: - """Compatibility wrapper for legacy robot-scoped Viser callers.""" - if self._closed: - return - self._ensure_started() - if self._scene is not None: - self._scene.animate_path(str(robot_id), path, duration) - def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: if self._closed: return self._ensure_started() if self._scene is None: return - for robot_name in self._robot_names_for_groups(plan.group_ids): + preview = self._build_group_preview_animation(plan) + if preview is not None: + self._scene.animate_preview(preview, duration) + + def _build_group_preview_animation(self, plan: GeneratedPlan) -> GroupPreviewAnimation | None: + selection = self._world_monitor.planning_groups.select(plan.group_ids) + tracks: list[PreviewTrack] = [] + for robot_name in selection.robot_names: robot_id = self._manipulation_module.robot_id_for_name(robot_name) config = self._manipulation_module.get_robot_config(robot_name) - current = self._manipulation_module.get_current_joint_state(robot_name) + get_current_joint_state = getattr( + self._manipulation_module, "get_current_joint_state", None + ) + current = ( + get_current_joint_state(robot_name) + if callable(get_current_joint_state) + else self._world_monitor.get_current_joint_state(robot_id) + if robot_id is not None + else None + ) if robot_id is None or config is None or current is None: logger.warning( "Cannot build group preview for robot '%s': missing id, config, or state", robot_name, ) - return + return None path = self._robot_path_for_plan(robot_name, config, current, plan) if not path: logger.warning("Cannot project generated plan for robot '%s'", robot_name) - return - self._scene.animate_path(str(robot_id), path, duration) - - def _robot_names_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: - selection = self._world_monitor.planning_groups.select(group_ids) - return list(selection.robot_names) + return None + tracks.append( + PreviewTrack( + robot_id=str(robot_id), + group_ids=tuple( + group.id for group in selection.groups if group.robot_name == robot_name + ), + joint_names=tuple(config.joint_names), + path=tuple(path), + ) + ) + if not tracks: + return None + return GroupPreviewAnimation(group_ids=plan.group_ids, tracks=tuple(tracks)) def _robot_ids_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: - if isinstance(group_ids, str): - return [group_ids] - if not hasattr(self._world_monitor, "planning_groups"): - return [str(group_id) for group_id in group_ids] selection = self._world_monitor.planning_groups.select(group_ids) robot_ids: list[str] = [] for robot_name in selection.robot_names: @@ -299,7 +310,6 @@ def close(self) -> None: errors.append(e) self._runtime = None self._server = None - self._adapter = None self._scene = None self._gui = None if errors: