Jalaj bt pickplace

dimos/core/rpc_client.py

@@ -62,12 +62,16 @@ def __call__(self, *args, **kwargs):  # type: ignore[no-untyped-def]
         # For stop, use call_nowait to avoid deadlock
         # (the remote side stops its RPC service before responding)
         if self._name == "stop":
-            self._rpc.call_nowait(f"{self._remote_name}/{self._name}", (args, kwargs))  # type: ignore[arg-type]
+            self._rpc.call_nowait(f"{self._remote_name}/{self._name}", (list(args), kwargs))  # type: ignore[arg-type]
             if self._stop_rpc_client:
                 self._stop_rpc_client()
             return None
 
-        result, unsub_fn = self._rpc.call_sync(f"{self._remote_name}/{self._name}", (args, kwargs))  # type: ignore[arg-type]
+        timeout = kwargs.pop("rpc_timeout", None)
+        sync_args: dict[str, float] = {"rpc_timeout": timeout} if timeout is not None else {}
+        result, unsub_fn = self._rpc.call_sync(
+            f"{self._remote_name}/{self._name}", (list(args), kwargs), **sync_args
+        )
         self._unsub_fns.append(unsub_fn)
         return result
 

dimos/hardware/manipulators/xarm/adapter.py

@@ -20,11 +20,14 @@
 
 from __future__ import annotations
 
+import logging
 import math
 from typing import TYPE_CHECKING
 
 from xarm.wrapper import XArmAPI
 
+logger = logging.getLogger(__name__)
+
 if TYPE_CHECKING:
     from dimos.hardware.manipulators.registry import AdapterRegistry
 
@@ -74,6 +77,10 @@ def connect(self) -> bool:
                 print(f"ERROR: XArm at {self._ip} not reachable (connected=False)")
                 return False
 
+            # Clear any stale errors and enable motion before setting mode
+            self._arm.clean_error()
+            self._arm.motion_enable(enable=True)
+
             # Initialize to servo mode for high-frequency control
             self._arm.set_mode(_XARM_MODE_SERVO_CARTESIAN)  # Mode 1 = servo mode
             self._arm.set_state(0)
@@ -221,6 +228,12 @@ def write_joint_positions(
         # Use set_servo_angle_j for high-frequency servo control (100Hz+)
         # This only executes the last instruction, suitable for real-time control
         code: int = self._arm.set_servo_angle_j(angles, speed=100, mvacc=500)
+        if code != 0:
+            logger.error(
+                f"[XArm] set_servo_angle_j failed: code={code}, "
+                f"error_code={self._arm.error_code}, warn_code={self._arm.warn_code}, "
+                f"state={self._arm.state}, mode={self._arm.mode}"
+            )
         return code == 0
 
     def write_joint_velocities(self, velocities: list[float]) -> bool:

dimos/manipulation/blueprints.py

@@ -37,6 +37,8 @@
 from dimos.core.blueprints import autoconnect
 from dimos.core.transport import LCMTransport
 from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
+from dimos.manipulation.bt.bt_manipulation_module import bt_manipulation_module
+from dimos.manipulation.bt.pick_place_module import PickPlaceModule
 from dimos.manipulation.manipulation_module import ManipulationModule
 from dimos.manipulation.pick_and_place_module import PickAndPlaceModule
 from dimos.manipulation.planning.spec.config import RobotModelConfig
@@ -45,6 +47,7 @@
 from dimos.msgs.geometry_msgs.Transform import Transform
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.JointState import JointState
+from dimos.perception.detection.detectors.yoloe import YoloePromptMode
 from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule
 from dimos.robot.foxglove_bridge import FoxgloveBridge  # TODO: migrate to rerun
 from dimos.utils.data import get_data
@@ -210,6 +213,13 @@ def _make_xarm7_config(
     if add_gripper:
         xacro_args["add_gripper"] = "true"
 
+    # xArm7 joint limits from URDF (xarm7.urdf.xacro with limited=true)
+    # Without these, the planner defaults to +- pi which rejects valid joint
+    # positions beyond 3.14 rad (joints 1,3,5,7 go to +- 2Pi).
+    _TWO_PI = 2.0 * math.pi
+    joint_limits_lower = [-_TWO_PI, -2.059, -_TWO_PI, -0.19198, -_TWO_PI, -1.69297, -_TWO_PI]
+    joint_limits_upper = [_TWO_PI, 2.0944, _TWO_PI, 3.927, _TWO_PI, math.pi, _TWO_PI]
+
     return RobotModelConfig(
         name=name,
         urdf_path=_get_xarm_urdf_path(),
@@ -227,6 +237,8 @@ def _make_xarm7_config(
         coordinator_task_name=coordinator_task,
         gripper_hardware_id=gripper_hardware_id,
         tf_extra_links=tf_extra_links or [],
+        joint_limits_lower=joint_limits_lower,
+        joint_limits_upper=joint_limits_upper,
         # 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],
     )
@@ -403,12 +415,16 @@ def _make_piper_config(
             ],
             planning_timeout=10.0,
             enable_viz=True,
+            graspgen_topk_num_grasps=400,
+            graspgen_save_visualization_data=True,
         ),
         RealSenseCamera.blueprint(
             base_frame_id="link7",
             base_transform=_XARM_PERCEPTION_CAMERA_TRANSFORM,
         ),
-        ObjectSceneRegistrationModule.blueprint(target_frame="world"),
+        ObjectSceneRegistrationModule.blueprint(
+            target_frame="world", prompt_mode=YoloePromptMode.PROMPT
+        ),
         FoxgloveBridge.blueprint(),  # TODO: migrate to rerun
     )
     .transports(
@@ -458,10 +474,81 @@ def _make_piper_config(
     McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT),
 )
 
+_BT_AGENT_SYSTEM_PROMPT = """\
+You are a robotic manipulation assistant controlling an xArm7 robot arm.
+
+Available skills:
+- pick: Pick up an object by name. Uses DL-based grasp generation with retry and verification. \
+The BT automatically scans and detects the object.
+- place: Place a held object at x, y, z position (meters, world frame).
+- place_back: ONLY when user says "put it back" or "return the object". \
+Releases the object at its original pick position.
+- pick_and_place: Pick an object and place it at target location.
+- go_home: Move arm to home/safe position. Use for "come back", "go back", \
+"return home", "come here". Keeps held object — does NOT release it.
+- stop: Emergency stop — cancels motion and opens gripper.
+
+IMPORTANT: "come back", "go back", "return" = go_home (keeps object). \
+"put it back", "return the object" = place_back (releases object). \
+When in doubt, use go_home.
+
+Workflow: Just call pick with the object name — the BT handles scanning automatically. \
+If the user mentions a named location (e.g. "the box"), ask for or remember its \
+coordinates from conversation and use place(x, y, z) directly. \
+Do NOT sequence low-level operations. Use pick/place for all manipulation.
+For robot_name parameters, always omit or pass None (single-arm setup).
+"""
+
+# BT Agent mode — uses BTManipulationModule (no skills) + BT PickPlaceModule (skills only)
+# DL grasps via GraspGen Docker (requires GPU + Docker build)
+_bt_xarm_perception = (
+    autoconnect(
+        bt_manipulation_module(
+            robots=[
+                _make_xarm7_config(
+                    "arm",
+                    pitch=math.radians(45),
+                    joint_prefix="arm_",
+                    coordinator_task="traj_arm",
+                    add_gripper=True,
+                    gripper_hardware_id="arm",
+                    tf_extra_links=["link7"],
+                ),
+            ],
+            planning_timeout=10.0,
+            enable_viz=True,
+            graspgen_topk_num_grasps=400,
+            graspgen_save_visualization_data=True,
+        ),
+        RealSenseCamera.blueprint(
+            base_frame_id="link7",
+            base_transform=_XARM_PERCEPTION_CAMERA_TRANSFORM,
+        ),
+        ObjectSceneRegistrationModule.blueprint(
+            target_frame="world", prompt_mode=YoloePromptMode.PROMPT
+        ),
+        FoxgloveBridge.blueprint(),  # TODO: migrate to rerun
+    )
+    .transports(
+        {
+            ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+        }
+    )
+    .global_config(viewer="foxglove")
+)
+
+bt_pick_place_agent = autoconnect(
+    _bt_xarm_perception,
+    PickPlaceModule.blueprint(),
+    McpServer.blueprint(),
+    McpClient.blueprint(system_prompt=_BT_AGENT_SYSTEM_PROMPT),
+)
+
 
 __all__ = [
     "PIPER_GRIPPER_COLLISION_EXCLUSIONS",
     "XARM_GRIPPER_COLLISION_EXCLUSIONS",
+    "bt_pick_place_agent",
     "dual_xarm6_planner",
     "xarm6_planner_only",
     "xarm7_planner_coordinator",