Merge branch 'dev' into ruthwik_iphone_teleop

Author: ruthwikdasyam

dimos/core/__init__.py

@@ -5,11 +5,11 @@
 import time
 from typing import TYPE_CHECKING, cast
 
+import lazy_loader as lazy
 from rich.console import Console
 
 from dimos.core.core import rpc
 from dimos.utils.logging_config import setup_logger
-import lazy_loader as lazy
 
 if TYPE_CHECKING:
     # Avoid runtime import to prevent circular import; ruff's TC001 would otherwise move it.

dimos/manipulation/control/coordinator_client.py

@@ -123,20 +123,25 @@ def get_joint_positions(self) -> dict[str, float]:
         return self._rpc.get_joint_positions() or {}
 
     def get_trajectory_status(self, task_name: str) -> dict[str, Any]:
-        """Get status of a trajectory task."""
-        return self._rpc.get_trajectory_status(task_name) or {}
+        """Get status of a trajectory task via task_invoke."""
+        result = self._rpc.task_invoke(task_name, "get_state", {})
+        if result is not None:
+            return {"state": int(result), "task": task_name}
+        return {}
 
     # =========================================================================
-    # Trajectory execution (RPC calls)
+    # Trajectory execution (via task_invoke)
     # =========================================================================
 
     def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> bool:
-        """Execute a trajectory on a task."""
-        return self._rpc.execute_trajectory(task_name, trajectory) or False
+        """Execute a trajectory on a task via task_invoke."""
+        result = self._rpc.task_invoke(task_name, "execute", {"trajectory": trajectory})
+        return bool(result)
 
     def cancel_trajectory(self, task_name: str) -> bool:
-        """Cancel an active trajectory."""
-        return self._rpc.cancel_trajectory(task_name) or False
+        """Cancel an active trajectory via task_invoke."""
+        result = self._rpc.task_invoke(task_name, "cancel", {})
+        return bool(result)
 
     # =========================================================================
     # Task selection and setup
@@ -320,26 +325,32 @@ def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> N
 
 
 def wait_for_completion(client: CoordinatorClient, task_name: str, timeout: float = 60.0) -> bool:
-    """Wait for trajectory to complete with progress display."""
+    """Wait for trajectory to complete by polling task state.
+
+    TrajectoryState is an IntEnum: IDLE=0, EXECUTING=1, COMPLETED=2, ABORTED=3, FAULT=4.
+    """
     start = time.time()
-    last_progress = -1.0
+    _STATE_NAMES = {0: "IDLE", 1: "EXECUTING", 2: "COMPLETED", 3: "ABORTED", 4: "FAULT"}
 
     while time.time() - start < timeout:
         status = client.get_trajectory_status(task_name)
-        if not status.get("active", False):
-            state: str = status.get("state", "UNKNOWN")
-            print(f"\nTrajectory finished: {state}")
-            return state == "COMPLETED"
+        if not status:
+            print("\nCould not get trajectory status")
+            return False
 
-        progress = status.get("progress", 0.0)
-        if progress != last_progress:
-            bar_len = 30
-            filled = int(bar_len * progress)
-            bar = "=" * filled + "-" * (bar_len - filled)
-            print(f"\r[{bar}] {progress * 100:.1f}%", end="", flush=True)
-            last_progress = progress
+        state_val = status.get("state")
+        state_name = _STATE_NAMES.get(state_val, f"UNKNOWN({state_val})")  # type: ignore[arg-type]
 
-        time.sleep(0.05)
+        if state_val in (0, 2):  # IDLE or COMPLETED
+            print(f"\nTrajectory finished: {state_name}")
+            return True
+        if state_val in (3, 4):  # ABORTED or FAULT
+            print(f"\nTrajectory failed: {state_name}")
+            return False
+        # state_val == 1 means EXECUTING, keep polling
+        elapsed = time.time() - start
+        print(f"\r  Executing... ({elapsed:.1f}s)", end="", flush=True)
+        time.sleep(0.1)
 
     print("\nTimeout waiting for trajectory")
     return False
@@ -471,12 +482,12 @@ def run(self) -> None:
 
     def status(self) -> None:
         """Show task status."""
+        _STATE_NAMES = {0: "IDLE", 1: "EXECUTING", 2: "COMPLETED", 3: "ABORTED", 4: "FAULT"}
         status = self._client.get_trajectory_status(self._current_task)
+        state_val = status.get("state")
+        state_name = _STATE_NAMES.get(state_val, f"UNKNOWN({state_val})")  # type: ignore[arg-type]
         print(f"\nTask: {self._current_task}")
-        print(f"  Active: {status.get('active', False)}")
-        print(f"  State: {status.get('state', 'UNKNOWN')}")
-        if "progress" in status:
-            print(f"  Progress: {status['progress'] * 100:.1f}%")
+        print(f"  State: {state_name} ({state_val})")
 
     def cancel(self) -> None:
         """Cancel active trajectory."""

dimos/manipulation/manipulation_blueprints.py

@@ -16,20 +16,19 @@
 Blueprints for manipulation module integration with ControlCoordinator.
 
 Usage:
-    # Start coordinator first, then planner:
-    coordinator = xarm7_planner_coordinator.build()
-    coordinator.loop()
-
-    # Plan and execute via RPC client:
-    from dimos.manipulation.planning.examples.manipulation_client import ManipulationClient
-    client = ManipulationClient()
-    client.plan_to_joints([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
-    client.execute()
+    # Non-agentic (manual RPC):
+    dimos run coordinator-mock
+    dimos run xarm-perception
+
+    # Agentic (LLM agent with skills):
+    dimos run coordinator-mock
+    dimos run xarm-perception-agent
 """
 
 import math
 from pathlib import Path
 
+from dimos.agents.agent import Agent
 from dimos.core.blueprints import autoconnect
 from dimos.core.transport import LCMTransport
 from dimos.hardware.sensors.camera.realsense import realsense_camera
@@ -176,6 +175,7 @@ def _make_xarm6_config(
         max_acceleration=2.0,
         joint_name_mapping=joint_mapping,
         coordinator_task_name=coordinator_task,
+        home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     )
 
 
@@ -232,6 +232,8 @@ def _make_xarm7_config(
         coordinator_task_name=coordinator_task,
         gripper_hardware_id=gripper_hardware_id,
         tf_extra_links=tf_extra_links or [],
+        # Home configuration: arm extended forward, elbow up (safe observe pose)
+        home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     )
 
 
@@ -272,6 +274,7 @@ def _make_piper_config(
         max_acceleration=2.0,
         joint_name_mapping=joint_mapping,
         coordinator_task_name=coordinator_task,
+        home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     )
 
 
@@ -366,11 +369,42 @@ def _make_piper_config(
 )
 
 
+# XArm7 perception + LLM agent for agentic manipulation
+# Skills (pick, place, move_to_pose, etc.) auto-register with the agent's SkillCoordinator.
+# Usage: dimos run coordinator-mock, then dimos run xarm-perception-agent
+_MANIPULATION_AGENT_SYSTEM_PROMPT = """\
+You are a robotic manipulation assistant controlling an xArm7 robot arm.
+
+Use ONLY these ManipulationModule skills for manipulation tasks:
+- scan_objects: Scan scene and list detected objects with 3D positions. Always call this first.
+- pick: Pick up an object by name. Requires scan_objects first.
+- place: Place a held object at x, y, z position.
+- place_back: Place a held object back at its original pick position.
+- pick_and_place: Pick an object and place it at a target location.
+- move_to_pose: Move end-effector to x, y, z with optional roll, pitch, yaw.
+- 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.
+- get_scene_info: Get full robot state, detected objects, and scene info.
+
+Do NOT use the 'detect' or 'select' skills — use scan_objects instead.
+For robot_name parameters, always omit or pass None (single-arm setup).
+After pick or place, return to init with go_init unless another action follows immediately.
+"""
+
+xarm_perception_agent = autoconnect(
+    xarm_perception,
+    Agent.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT),
+)
+
+
 __all__ = [
     "PIPER_GRIPPER_COLLISION_EXCLUSIONS",
     "XARM_GRIPPER_COLLISION_EXCLUSIONS",
     "dual_xarm6_planner",
     "xarm6_planner_only",
     "xarm7_planner_coordinator",
     "xarm_perception",
+    "xarm_perception_agent",
 ]