diff --git a/dimos/conftest.py b/dimos/conftest.py index 4ab8a401f8..8a156e4f72 100644 --- a/dimos/conftest.py +++ b/dimos/conftest.py @@ -74,6 +74,8 @@ def event_loop(): @pytest.fixture(scope="session", autouse=True) def _autoconf(request): """Run autoconf() before all tests with capture suspended so people see `sudo` commands.""" + if os.environ.get("DIMOS_SKIP_AUTOCONF"): + return capman = request.config.pluginmanager.getplugin("capturemanager") capman.suspend_global_capture(in_=True) diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 97657b9cae..991c1e43e4 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -42,8 +42,8 @@ from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs import JointState from dimos.perception.object_scene_registration import object_scene_registration_module -from dimos.robot.foxglove_bridge import foxglove_bridge # TODO: migrate to rerun from dimos.utils.data import get_data +from dimos.visualization.rerun.bridge import rerun_bridge # ============================================================================= # Pose Helpers @@ -423,14 +423,16 @@ def _make_piper_config( base_transform=_XARM_PERCEPTION_CAMERA_TRANSFORM, ), object_scene_registration_module(target_frame="world"), - foxglove_bridge(), # TODO: migrate to rerun + rerun_bridge( + min_interval_sec=0.5, # 2 Hz cap for Image/PointCloud2 streams to keep Rerun stable. + ), ) .transports( { ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), } ) - .global_config(viewer="foxglove") + .global_config(viewer="rerun") ) diff --git a/dimos/manipulation/bt/__init__.py b/dimos/manipulation/bt/__init__.py new file mode 100644 index 0000000000..724c67c1f8 --- /dev/null +++ b/dimos/manipulation/bt/__init__.py @@ -0,0 +1,5 @@ +"""Internal behavior-tree orchestration for manipulation.""" + +from dimos.manipulation.bt.trees import RetryOnFailure, build_pick_tree, build_place_tree + +__all__ = ["RetryOnFailure", "build_pick_tree", "build_place_tree"] diff --git a/dimos/manipulation/bt/actions.py b/dimos/manipulation/bt/actions.py new file mode 100644 index 0000000000..b9885baf65 --- /dev/null +++ b/dimos/manipulation/bt/actions.py @@ -0,0 +1,506 @@ +"""Behavior-tree action nodes for current PickAndPlaceModule internals.""" + +from __future__ import annotations + +import math +import time +from typing import TYPE_CHECKING + +import py_trees +from py_trees.common import Status + +from dimos.msgs.geometry_msgs import Pose, Quaternion, Vector3 +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.manipulation.pick_and_place_module import PickAndPlaceModule + +logger = setup_logger() + + +class ManipulationAction(py_trees.behaviour.Behaviour): + """Base action node with shared blackboard keys.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name=name) + self.module = module + self.bb = self.attach_blackboard_client(name=self.name) + self.bb.register_key(key="error_message", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="result_message", access=py_trees.common.Access.WRITE) + + +class ClearGraspState(ManipulationAction): + """Clear per-attempt grasp state.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + for key in ( + "target_object", + "object_pointcloud", + "scene_pointcloud", + "grasp_candidates", + "grasp_index", + "current_grasp", + "pre_grasp_pose", + "lift_pose", + "has_object", + "grasp_source", + ): + self.bb.register_key(key=key, access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + self.bb.target_object = None + self.bb.object_pointcloud = None + self.bb.scene_pointcloud = None + self.bb.grasp_candidates = [] + self.bb.grasp_index = 0 + self.bb.current_grasp = None + self.bb.pre_grasp_pose = None + self.bb.lift_pose = None + self.bb.has_object = False + self.bb.grasp_source = "" + return Status.SUCCESS + + +class ScanObjects(ManipulationAction): + """Refresh perception detections.""" + + def __init__(self, name: str, module: PickAndPlaceModule, min_duration: float) -> None: + super().__init__(name, module) + self.min_duration = min_duration + self.bb.register_key(key="detections", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + try: + self.module.refresh_obstacles(self.min_duration) + self.bb.detections = list(self.module._detection_snapshot) + except Exception as e: + self.bb.error_message = f"Error: Scan failed - {e}" + logger.error(f"[ScanObjects] {e}") + return Status.FAILURE + return Status.SUCCESS if self.bb.detections else Status.FAILURE + + +class ClearPerceptionObstacles(ManipulationAction): + """Clear perception obstacles from the planning world.""" + + def update(self) -> Status: + try: + result = self.module.clear_perception_obstacles() + logger.info(f"[ClearPerceptionObstacles] {result}") + except Exception as e: + self.bb.error_message = f"Error: Clear perception obstacles failed - {e}" + logger.error(f"[ClearPerceptionObstacles] {e}") + return Status.FAILURE + return Status.SUCCESS + + +class ResetRobot(ManipulationAction): + """Cancel active motion if possible and reset the module state.""" + + def update(self) -> Status: + try: + self.module.cancel() + except Exception: + logger.warning("[ResetRobot] cancel failed", exc_info=True) + result = self.module.reset() + if isinstance(result, str) and result.startswith("Error:"): + self.bb.error_message = result + return Status.FAILURE + return Status.SUCCESS + + +class CancelMotion(ManipulationAction): + """Best-effort cancellation of active motion.""" + + def update(self) -> Status: + try: + self.module.cancel() + except Exception: + logger.warning("[CancelMotion] cancel failed", exc_info=True) + return Status.SUCCESS + + +class FindObject(ManipulationAction): + """Find the target object in the module's detection snapshot.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="object_name", access=py_trees.common.Access.READ) + self.bb.register_key(key="object_id", access=py_trees.common.Access.READ) + self.bb.register_key(key="target_object", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + det = self.module._find_object_in_detections(self.bb.object_name, self.bb.object_id) + if det is None: + self.bb.error_message = f"Error: Object '{self.bb.object_name}' not found" + return Status.FAILURE + self.bb.target_object = det + return Status.SUCCESS + + +class GetObjectPointcloud(ManipulationAction): + """Best-effort object point-cloud fetch through OSR RPCs.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="target_object", access=py_trees.common.Access.READ) + self.bb.register_key(key="object_pointcloud", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + target = self.bb.target_object + obj_id = getattr(target, "object_id", None) + obj_name = getattr(target, "name", "") + try: + if obj_id: + pc = self.module.get_rpc_calls( + "ObjectSceneRegistrationModule.get_object_pointcloud_by_object_id" + )(obj_id) + else: + pc = self.module.get_rpc_calls( + "ObjectSceneRegistrationModule.get_object_pointcloud_by_name" + )(obj_name) + except Exception as e: + logger.warning(f"[GetObjectPointcloud] unavailable: {e}") + self.bb.error_message = f"Error: Object pointcloud unavailable - {e}" + return Status.FAILURE + + if pc is None: + self.bb.error_message = f"Error: No pointcloud for '{obj_name}'" + return Status.FAILURE + self.bb.object_pointcloud = pc + return Status.SUCCESS + + +class GetScenePointcloud(ManipulationAction): + """Best-effort scene point-cloud fetch. Missing scene cloud is non-fatal.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="target_object", access=py_trees.common.Access.READ) + self.bb.register_key(key="scene_pointcloud", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + try: + obj_id = getattr(self.bb.target_object, "object_id", None) + self.bb.scene_pointcloud = self.module.get_rpc_calls( + "ObjectSceneRegistrationModule.get_full_scene_pointcloud" + )(exclude_object_id=obj_id) + except Exception as e: + logger.warning(f"[GetScenePointcloud] unavailable: {e}") + self.bb.scene_pointcloud = None + return Status.SUCCESS + + +class GenerateGrasps(ManipulationAction): + """Generate GraspGen candidates from object and scene point clouds.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="object_pointcloud", access=py_trees.common.Access.READ) + self.bb.register_key(key="scene_pointcloud", access=py_trees.common.Access.READ) + self.bb.register_key(key="grasp_candidates", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="grasp_index", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="grasp_source", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + result = self.module.generate_grasps( + self.bb.object_pointcloud, + getattr(self.bb, "scene_pointcloud", None), + ) + poses = list(getattr(result, "poses", []) or []) + if not poses: + self.bb.error_message = "Error: GraspGen returned no grasps" + return Status.FAILURE + self.bb.grasp_candidates = poses + self.bb.grasp_index = 0 + self.bb.grasp_source = "graspgen" + logger.info(f"[GenerateGrasps] Generated {len(poses)} GraspGen candidates") + return Status.SUCCESS + + +class GenerateHeuristicGrasps(ManipulationAction): + """Fallback top-down grasps above the detection center.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="target_object", access=py_trees.common.Access.READ) + self.bb.register_key(key="grasp_candidates", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="grasp_index", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="grasp_source", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + target = self.bb.target_object + center = getattr(target, "center", None) + if center is None: + self.bb.error_message = "Error: No detection center for heuristic grasp" + return Status.FAILURE + z_offsets = getattr( + self.module.config, + "bt_heuristic_grasp_z_offsets", + (0.08, 0.12, 0.16), + ) + min_heuristic_z = getattr( + self.module.config, + "bt_min_heuristic_grasp_z", + self.module.config.bt_min_grasp_z, + ) + candidates: list[Pose] = [] + seen_z: set[float] = set() + for z_offset in z_offsets: + z = max( + center.z + z_offset, + self.module.config.bt_min_grasp_z, + min_heuristic_z, + ) + z_key = round(z, 2) + if z_key in seen_z: + continue + seen_z.add(z_key) + candidates.append( + Pose( + Vector3(center.x, center.y, z), + Quaternion.from_euler(Vector3(0.0, math.pi, 0.0)), + ) + ) + self.bb.grasp_candidates = candidates + self.bb.grasp_index = 0 + self.bb.grasp_source = "heuristic" + zs = ", ".join(f"{pose.position.z:.3f}" for pose in candidates) + logger.info( + f"[GenerateHeuristicGrasps] {len(candidates)} top-down grasps above " + f"({center.x:.3f}, {center.y:.3f}, {center.z:.3f}); z=[{zs}]" + ) + return Status.SUCCESS + + +class FilterGraspWorkspace(ManipulationAction): + """Filter candidates by simple workspace constraints.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="grasp_candidates", access=py_trees.common.Access.READ) + self.bb.register_key(key="grasp_candidates", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="grasp_index", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + cfg = self.module.config + candidates = list(self.bb.grasp_candidates) + filtered: list[Pose] = [] + cos_threshold = math.cos(cfg.bt_max_approach_angle) + + for pose in candidates: + if pose.position.z < cfg.bt_min_grasp_z: + logger.info( + "[FilterGraspWorkspace] rejected by z: " + f"z={pose.position.z:.3f} min={cfg.bt_min_grasp_z:.3f}" + ) + continue + distance = pose.position.magnitude() + if distance > cfg.bt_max_grasp_distance: + logger.info( + "[FilterGraspWorkspace] rejected by distance: " + f"distance={distance:.3f} max={cfg.bt_max_grasp_distance:.3f}" + ) + continue + qx, qy = pose.orientation.x, pose.orientation.y + approach_z = 1.0 - 2.0 * (qx * qx + qy * qy) + if approach_z > -cos_threshold: + logger.info( + "[FilterGraspWorkspace] rejected by approach angle: " + f"approach_z={approach_z:.3f} threshold={-cos_threshold:.3f}" + ) + continue + filtered.append(pose) + + if not filtered: + self.bb.error_message = "Error: No grasp candidates passed workspace filtering" + return Status.FAILURE + self.bb.grasp_candidates = filtered + self.bb.grasp_index = 0 + logger.info(f"[FilterGraspWorkspace] {len(filtered)}/{len(candidates)} passed") + return Status.SUCCESS + + +class SelectNextGrasp(ManipulationAction): + """Select the next candidate.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="grasp_candidates", access=py_trees.common.Access.READ) + self.bb.register_key(key="grasp_index", access=py_trees.common.Access.READ) + self.bb.register_key(key="grasp_index", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="current_grasp", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + candidates: list[Pose] = self.bb.grasp_candidates + idx: int = self.bb.grasp_index + if idx >= len(candidates): + self.bb.error_message = "Error: All grasp candidates exhausted" + return Status.FAILURE + self.bb.current_grasp = candidates[idx] + self.bb.grasp_index = idx + 1 + logger.info(f"[SelectNextGrasp] Candidate {idx + 1}/{len(candidates)}") + return Status.SUCCESS + + +class ComputePreGrasp(ManipulationAction): + """Compute the pre-grasp pose.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="current_grasp", access=py_trees.common.Access.READ) + self.bb.register_key(key="pre_grasp_pose", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + robot = self.module._get_robot(getattr(self.bb, "robot_name", None)) + robot_config = robot[2] if robot is not None else None + offset = getattr(robot_config, "pre_grasp_offset", 0.10) + self.bb.pre_grasp_pose = self.module._compute_pre_grasp_pose( + self.bb.current_grasp, offset + ) + p = self.bb.pre_grasp_pose.position + logger.info( + f"[ComputePreGrasp] offset={offset:.3f} pose=({p.x:.3f}, {p.y:.3f}, {p.z:.3f})" + ) + return Status.SUCCESS + + +class PlanToPose(ManipulationAction): + """Plan to a pose stored on the blackboard.""" + + def __init__(self, name: str, module: PickAndPlaceModule, pose_key: str) -> None: + super().__init__(name, module) + self.pose_key = pose_key + self.bb.register_key(key=pose_key, access=py_trees.common.Access.READ) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + pose = getattr(self.bb, self.pose_key) + robot_name = getattr(self.bb, "robot_name", None) + p = pose.position + logger.info( + f"[PlanToPose] Planning to {self.pose_key}: " + f"({p.x:.3f}, {p.y:.3f}, {p.z:.3f})" + ) + if self.module.get_state() == "FAULT": + self.module.reset() + if self.module.plan_to_pose(pose, robot_name): + logger.info(f"[PlanToPose] Planning to {self.pose_key} succeeded") + return Status.SUCCESS + self.bb.error_message = f"Error: Planning to {self.pose_key} failed" + logger.info(f"[PlanToPose] Planning to {self.pose_key} failed") + return Status.FAILURE + + +class ExecuteTrajectory(ManipulationAction): + """Preview, execute, and wait for completion using the current module helper.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + err = self.module._preview_execute_wait(getattr(self.bb, "robot_name", None)) + if err: + self.bb.error_message = err + return Status.FAILURE + return Status.SUCCESS + + +class SetGripper(ManipulationAction): + """Set gripper and wait for settle.""" + + def __init__( + self, + name: str, + module: PickAndPlaceModule, + position: float, + settle_time: float, + ) -> None: + super().__init__(name, module) + self.position = position + self.settle_time = settle_time + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + if not self.module._set_gripper_position(self.position, getattr(self.bb, "robot_name", None)): + self.bb.error_message = "Error: Gripper command failed" + return Status.FAILURE + time.sleep(self.settle_time) + return Status.SUCCESS + + +class ComputeLiftPose(ManipulationAction): + """Compute a straight-up lift from the intended grasp pose.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="current_grasp", access=py_trees.common.Access.READ) + self.bb.register_key(key="lift_pose", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + base = self.bb.current_grasp + self.bb.lift_pose = Pose( + Vector3(base.position.x, base.position.y, base.position.z + self.module.config.bt_lift_height), + base.orientation, + ) + p = self.bb.lift_pose.position + logger.info(f"[ComputeLiftPose] pose=({p.x:.3f}, {p.y:.3f}, {p.z:.3f})") + return Status.SUCCESS + + +class StorePickPosition(ManipulationAction): + """Remember successful pick position for place_back.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="current_grasp", access=py_trees.common.Access.READ) + + def update(self) -> Status: + self.module._last_pick_position = self.bb.current_grasp.position + return Status.SUCCESS + + +class ComputePlacePose(ManipulationAction): + """Compute place and pre-place poses.""" + + def __init__(self, name: str, module: PickAndPlaceModule, x: float, y: float, z: float) -> None: + super().__init__(name, module) + self.x = x + self.y = y + self.z = z + self.bb.register_key(key="place_pose", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="pre_place_pose", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + place_pose = Pose( + Vector3(self.x, self.y, self.z), + Quaternion.from_euler(Vector3(0.0, math.pi, 0.0)), + ) + self.bb.place_pose = place_pose + offset = getattr(self.module.config, "pre_grasp_offset", 0.10) + self.bb.pre_place_pose = self.module._compute_pre_grasp_pose( + place_pose, offset + ) + return Status.SUCCESS + + +class SetResultMessage(ManipulationAction): + """Write final result message.""" + + def __init__(self, name: str, module: PickAndPlaceModule, message: str) -> None: + super().__init__(name, module) + self.message = message + + def update(self) -> Status: + source = "" + try: + if self.bb.grasp_source: + source = f" using {self.bb.grasp_source} grasp" + except (AttributeError, KeyError): + pass + self.bb.result_message = self.message.format(grasp_source=source) + return Status.SUCCESS diff --git a/dimos/manipulation/bt/conditions.py b/dimos/manipulation/bt/conditions.py new file mode 100644 index 0000000000..48037f9253 --- /dev/null +++ b/dimos/manipulation/bt/conditions.py @@ -0,0 +1,123 @@ +"""Behavior-tree condition nodes for current PickAndPlaceModule internals.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import py_trees +from py_trees.common import Status + +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.manipulation.pick_and_place_module import PickAndPlaceModule + +logger = setup_logger() + + +class ManipulationCondition(py_trees.behaviour.Behaviour): + """Base condition node.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name=name) + self.module = module + self.bb = self.attach_blackboard_client(name=self.name) + + +class HasDetections(ManipulationCondition): + """Check whether the module has snapshotted detections.""" + + def update(self) -> Status: + return Status.SUCCESS if self.module._detection_snapshot else Status.FAILURE + + +class HasTargetDetection(ManipulationCondition): + """Check whether the requested target is already in the detection snapshot.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="object_name", access=py_trees.common.Access.READ) + self.bb.register_key(key="object_id", access=py_trees.common.Access.READ) + + def update(self) -> Status: + object_name = getattr(self.bb, "object_name", "") + object_id = getattr(self.bb, "object_id", None) + if not object_name and not object_id: + return Status.FAILURE + return ( + Status.SUCCESS + if self.module._find_object_in_detections(object_name, object_id) + else Status.FAILURE + ) + + +class RobotIsHealthy(ManipulationCondition): + """Check basic module and robot readiness.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + + def update(self) -> Status: + robot_name = getattr(self.bb, "robot_name", None) + if self.module.get_robot_info(robot_name) is None: + return Status.FAILURE + if self.module.get_state() in {"EXECUTING", "FAULT"}: + return Status.FAILURE + return Status.SUCCESS + + +class GripperHasObject(ManipulationCondition): + """Verify grasp by checking for a partially closed gripper.""" + + def __init__(self, name: str, module: PickAndPlaceModule) -> None: + super().__init__(name, module) + self.bb.register_key(key="robot_name", access=py_trees.common.Access.READ) + self.bb.register_key(key="has_object", access=py_trees.common.Access.WRITE) + self.bb.register_key(key="error_message", access=py_trees.common.Access.WRITE) + + def update(self) -> Status: + pos = self.module.get_gripper(getattr(self.bb, "robot_name", None)) + if pos is None: + self.bb.error_message = "Error: Gripper state unavailable" + self.bb.has_object = False + return Status.FAILURE + opening = float(pos) + min_opening = self.module.config.bt_gripper_grasp_threshold + max_opening = self.module.config.bt_gripper_grasp_max_open_position + if max_opening is None: + max_opening = ( + self.module.config.bt_gripper_open_position + * self.module.config.bt_gripper_grasp_max_open_fraction + ) + min_closure = self.module.config.bt_gripper_grasp_min_closure + if min_closure is not None: + open_reference = self.module.config.bt_gripper_grasp_open_reference + if open_reference is None: + open_reference = self.module.config.bt_gripper_open_position + max_opening = min(max_opening, open_reference - min_closure) + + logger.info( + f"[{self.name}] gripper opening={opening:.4f}m " + f"expected range=({min_opening:.4f}, {max_opening:.4f})m" + ) + + if min_opening < opening < max_opening: + self.bb.has_object = True + return Status.SUCCESS + self.bb.has_object = False + if opening >= max_opening: + self.bb.error_message = ( + "Error: Grasp verification failed - gripper still open " + f"({opening:.4f}m >= {max_opening:.4f}m)" + ) + else: + self.bb.error_message = ( + "Error: Grasp verification failed - gripper empty " + f"({opening:.4f}m <= {min_opening:.4f}m)" + ) + return Status.FAILURE + + +class VerifyHoldAfterLift(GripperHasObject): + """Re-check gripper after lift.""" diff --git a/dimos/manipulation/bt/test_pick_place_bt.py b/dimos/manipulation/bt/test_pick_place_bt.py new file mode 100644 index 0000000000..6fce7a3302 --- /dev/null +++ b/dimos/manipulation/bt/test_pick_place_bt.py @@ -0,0 +1,243 @@ +# 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 pick-and-place behavior-tree orchestration.""" + +from __future__ import annotations + +from dataclasses import dataclass +from types import SimpleNamespace +from typing import Any + +import py_trees +from py_trees.common import Status + +from dimos.manipulation.bt import actions +from dimos.manipulation.bt.conditions import GripperHasObject +from dimos.manipulation.bt.trees import RetryOnFailure, build_pick_tree, build_place_tree +from dimos.msgs.geometry_msgs import Vector3 + + +@dataclass +class _Config: + bt_tick_rate_hz: float = 1000.0 + bt_max_pick_attempts: int = 3 + bt_max_rescan_attempts: int = 2 + bt_enable_graspgen: bool = True + bt_scan_duration: float = 0.0 + bt_execution_timeout: float | None = None + bt_gripper_open_position: float = 0.85 + bt_gripper_close_position: float = 0.0 + bt_gripper_grasp_threshold: float = 0.005 + bt_gripper_grasp_max_open_fraction: float = 0.5 + bt_gripper_grasp_max_open_position: float | None = None + bt_gripper_grasp_open_reference: float | None = None + bt_gripper_grasp_min_closure: float | None = None + bt_gripper_settle_time: float = 0.0 + bt_lift_height: float = 0.10 + bt_min_grasp_z: float = 0.05 + bt_max_grasp_distance: float = 0.9 + bt_max_approach_angle: float = 1.05 + pre_grasp_offset: float = 0.10 + + +class _FakeModule: + def __init__(self) -> None: + self.config = _Config() + self._detection_snapshot = [ + SimpleNamespace(name="cup", object_id="obj-1", center=Vector3(0.3, 0.1, 0.2)) + ] + self._last_pick_position = None + self.gripper_position: float | None = 0.03 + + def _find_object_in_detections(self, object_name: str, object_id: str | None = None) -> Any: + for det in self._detection_snapshot: + if object_id == det.object_id or object_name in det.name: + return det + return None + + def get_rpc_calls(self, _name: str) -> Any: + raise RuntimeError("RPC unavailable in unit test") + + def generate_grasps(self, *_args: Any, **_kwargs: Any) -> Any: + return None + + def get_gripper(self, _robot_name: str | None = None) -> float | None: + return self.gripper_position + + +class _FailOnce(py_trees.behaviour.Behaviour): + def __init__(self) -> None: + super().__init__(name="FailOnce") + self.calls = 0 + + def update(self) -> Status: + self.calls += 1 + return Status.FAILURE if self.calls == 1 else Status.SUCCESS + + +def test_retry_on_failure_retries_child() -> None: + child = _FailOnce() + root = RetryOnFailure("Retry", child=child, max_attempts=2) + tree = py_trees.trees.BehaviourTree(root=root) + tree.setup() + + tree.tick() + assert root.status == Status.RUNNING + + tree.tick() + assert root.status == Status.SUCCESS + assert child.calls == 2 + + +def test_pick_tree_builds() -> None: + root = build_pick_tree( + module=_FakeModule(), # type: ignore[arg-type] + object_name="cup", + object_id=None, + robot_name=None, + ) + + assert root.name == "Pick" + assert [child.name for child in root.children] == [ + "EnsureReady", + "PickWithRescan", + "StorePickPosition", + "SetResult", + ] + + +def test_pick_tree_can_disable_graspgen() -> None: + module = _FakeModule() + module.config.bt_enable_graspgen = False + + root = build_pick_tree( + module=module, # type: ignore[arg-type] + object_name="cup", + object_id=None, + robot_name=None, + ) + + names = [node.name for node in root.iterate()] + assert "GraspGenCandidates" not in names + assert "GetObjectPointcloud" not in names + assert "GenerateGrasps" not in names + assert "GenerateGraspCandidates" in names + + +def test_place_tree_builds() -> None: + root = build_place_tree( + module=_FakeModule(), # type: ignore[arg-type] + x=0.4, + y=0.0, + z=0.2, + robot_name=None, + ) + + assert root.name == "Place" + assert root.children[0].name == "ComputePlacePose" + assert root.children[-1].name == "SetResult" + + +def test_graspgen_failure_falls_back_to_heuristic() -> None: + module = _FakeModule() + bb = py_trees.blackboard.Client(name="FallbackTestSetup") + for key in ("target_object", "grasp_candidates", "grasp_index", "grasp_source"): + bb.register_key(key=key, access=py_trees.common.Access.WRITE) + bb.target_object = module._detection_snapshot[0] + bb.grasp_candidates = [] + bb.grasp_index = 0 + bb.grasp_source = "" + + selector = py_trees.composites.Selector( + "GenerateGraspCandidates", + memory=True, + children=[ + py_trees.composites.Sequence( + "GraspGenCandidates", + memory=True, + children=[ + actions.GetObjectPointcloud( + "GetObjectPointcloud", module # type: ignore[arg-type] + ), + actions.GenerateGrasps("GenerateGrasps", module), # type: ignore[arg-type] + ], + ), + actions.GenerateHeuristicGrasps( + "HeuristicGrasps", module # type: ignore[arg-type] + ), + ], + ) + tree = py_trees.trees.BehaviourTree(root=selector) + tree.setup() + tree.tick() + + assert selector.status == Status.SUCCESS + assert bb.grasp_source == "heuristic" + assert len(bb.grasp_candidates) >= 1 + + +def test_gripper_check_accepts_partial_closure() -> None: + module = _FakeModule() + module.gripper_position = 0.03 + condition = GripperHasObject("VerifyGrasp", module) # type: ignore[arg-type] + tree = py_trees.trees.BehaviourTree(root=condition) + tree.setup() + tree.tick() + + assert condition.status == Status.SUCCESS + + +def test_gripper_check_rejects_fully_open_gripper() -> None: + module = _FakeModule() + module.gripper_position = module.config.bt_gripper_open_position + condition = GripperHasObject("VerifyGrasp", module) # type: ignore[arg-type] + tree = py_trees.trees.BehaviourTree(root=condition) + tree.setup() + tree.tick() + + assert condition.status == Status.FAILURE + + +def test_gripper_check_rejects_empty_closed_gripper() -> None: + module = _FakeModule() + module.gripper_position = 0.0 + condition = GripperHasObject("VerifyGrasp", module) # type: ignore[arg-type] + tree = py_trees.trees.BehaviourTree(root=condition) + tree.setup() + tree.tick() + + assert condition.status == Status.FAILURE + + +def test_gripper_check_uses_min_closure_from_open_reference() -> None: + module = _FakeModule() + module.config.bt_gripper_open_position = 0.85 + module.config.bt_gripper_grasp_max_open_fraction = 1.0 + module.config.bt_gripper_grasp_open_reference = 0.85 + module.config.bt_gripper_grasp_min_closure = 0.10 + + module.gripper_position = 0.80 + condition = GripperHasObject("VerifyGrasp", module) # type: ignore[arg-type] + tree = py_trees.trees.BehaviourTree(root=condition) + tree.setup() + tree.tick() + assert condition.status == Status.FAILURE + + module.gripper_position = 0.74 + condition = GripperHasObject("VerifyGrasp", module) # type: ignore[arg-type] + tree = py_trees.trees.BehaviourTree(root=condition) + tree.setup() + tree.tick() + assert condition.status == Status.SUCCESS diff --git a/dimos/manipulation/bt/trees.py b/dimos/manipulation/bt/trees.py new file mode 100644 index 0000000000..7797e4cc65 --- /dev/null +++ b/dimos/manipulation/bt/trees.py @@ -0,0 +1,251 @@ +"""Behavior-tree builders for pick-and-place.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import py_trees +from py_trees.common import Status + +from dimos.manipulation.bt import actions, conditions +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.manipulation.pick_and_place_module import PickAndPlaceModule + +logger = setup_logger() + + +class RetryOnFailure(py_trees.decorators.Decorator): + """Retry a child subtree on failure up to ``max_attempts``.""" + + def __init__(self, name: str, child: py_trees.behaviour.Behaviour, max_attempts: int) -> None: + super().__init__(name=name, child=child) + self.max_attempts = max_attempts + self._attempt = 0 + + def initialise(self) -> None: + self._attempt = 0 + + def update(self) -> Status: + if self.decorated.status == Status.SUCCESS: + return Status.SUCCESS + if self.decorated.status == Status.RUNNING: + return Status.RUNNING + + self._attempt += 1 + logger.info(f"[{self.name}] attempt {self._attempt}/{self.max_attempts} failed") + if self._attempt >= self.max_attempts: + return Status.FAILURE + + self.decorated.stop(Status.INVALID) + return Status.RUNNING + + +def build_pick_tree( + module: PickAndPlaceModule, + object_name: str, + object_id: str | None, + robot_name: str | None, +) -> py_trees.behaviour.Behaviour: + """Build a robust pick tree with GraspGen-first and heuristic fallback.""" + + cfg = module.config + + ensure_ready = py_trees.composites.Selector( + "EnsureReady", + memory=False, + children=[ + conditions.RobotIsHealthy("RobotIsHealthy", module), + py_trees.composites.Sequence( + "ResetAndVerify", + memory=True, + children=[ + actions.ResetRobot("ResetRobot", module), + conditions.RobotIsHealthy("RobotIsHealthyAfterReset", module), + ], + ), + ], + ) + + ensure_scanned = py_trees.composites.Selector( + "EnsureScanned", + memory=False, + children=[ + conditions.HasDetections("HasDetections", module), + actions.ScanObjects("ScanObjects", module, min_duration=cfg.bt_scan_duration), + ], + ) + + prepare_detections = py_trees.composites.Selector( + "PrepareDetections", + memory=False, + children=[ + conditions.HasTargetDetection("UseExistingTargetSnapshot", module), + py_trees.composites.Sequence( + "RefreshTargetSnapshot", + memory=True, + children=[ + actions.ClearPerceptionObstacles("ClearPerceptionObstacles", module), + ensure_scanned, + ], + ), + ], + ) + + if cfg.bt_enable_graspgen: + dl_grasps = py_trees.composites.Sequence( + "GraspGenCandidates", + memory=True, + children=[ + actions.GetObjectPointcloud("GetObjectPointcloud", module), + actions.GetScenePointcloud("GetScenePointcloud", module), + actions.GenerateGrasps("GenerateGrasps", module), + ], + ) + + generate_candidates: py_trees.behaviour.Behaviour = py_trees.composites.Selector( + "GenerateGraspCandidates", + memory=True, + children=[ + dl_grasps, + actions.GenerateHeuristicGrasps("HeuristicGrasps", module), + ], + ) + else: + generate_candidates = actions.GenerateHeuristicGrasps("GenerateGraspCandidates", module) + + grasp_attempt = py_trees.composites.Sequence( + "GraspAttempt", + memory=True, + children=[ + actions.SelectNextGrasp("SelectNextGrasp", module), + actions.ComputePreGrasp("ComputePreGrasp", module), + actions.SetGripper( + "OpenGripper", + module, + position=cfg.bt_gripper_open_position, + settle_time=0.5, + ), + actions.PlanToPose("PlanToPreGrasp", module, pose_key="pre_grasp_pose"), + actions.ExecuteTrajectory("ExecuteApproach", module), + actions.PlanToPose("PlanToGrasp", module, pose_key="current_grasp"), + actions.ExecuteTrajectory("ExecuteGrasp", module), + actions.SetGripper( + "CloseGripper", + module, + position=cfg.bt_gripper_close_position, + settle_time=cfg.bt_gripper_settle_time, + ), + conditions.GripperHasObject("VerifyGrasp", module), + actions.ComputeLiftPose("ComputeLiftPose", module), + actions.PlanToPose("PlanLift", module, pose_key="lift_pose"), + actions.ExecuteTrajectory("ExecuteLift", module), + conditions.VerifyHoldAfterLift("VerifyHoldAfterLift", module), + ], + ) + + recover_then_fail = py_trees.composites.Sequence( + "RecoverThenFail", + memory=True, + children=[ + actions.CancelMotion("CancelMotion", module), + actions.ResetRobot("ResetAfterFailure", module), + py_trees.behaviours.Failure("RecoveryComplete"), + ], + ) + + attempt_or_recover = py_trees.composites.Selector( + "AttemptOrRecover", + memory=True, + children=[grasp_attempt, recover_then_fail], + ) + + grasp_with_retry = RetryOnFailure( + "GraspWithRetry", + child=attempt_or_recover, + max_attempts=cfg.bt_max_pick_attempts, + ) + + scan_and_grasp = py_trees.composites.Sequence( + "ScanAndGrasp", + memory=True, + children=[ + actions.ClearGraspState("ClearGraspState", module), + prepare_detections, + actions.FindObject("FindObject", module), + generate_candidates, + actions.FilterGraspWorkspace("FilterGraspWorkspace", module), + grasp_with_retry, + ], + ) + + pick_with_rescan = RetryOnFailure( + "PickWithRescan", + child=scan_and_grasp, + max_attempts=cfg.bt_max_rescan_attempts, + ) + + root = py_trees.composites.Sequence( + "Pick", + memory=True, + children=[ + ensure_ready, + pick_with_rescan, + actions.StorePickPosition("StorePickPosition", module), + actions.SetResultMessage( + "SetResult", + module, + message=f"Pick complete - grasped '{object_name}' successfully{{grasp_source}}", + ), + ], + ) + + bb = py_trees.blackboard.Client(name="PickTreeSetup") + for key in ("object_name", "object_id", "robot_name"): + bb.register_key(key=key, access=py_trees.common.Access.WRITE) + bb.object_name = object_name + bb.object_id = object_id + bb.robot_name = robot_name + return root + + +def build_place_tree( + module: PickAndPlaceModule, + x: float, + y: float, + z: float, + robot_name: str | None, +) -> py_trees.behaviour.Behaviour: + """Build a place tree using current module planning/execution primitives.""" + + cfg = module.config + root = py_trees.composites.Sequence( + "Place", + memory=True, + children=[ + actions.ComputePlacePose("ComputePlacePose", module, x=x, y=y, z=z), + actions.PlanToPose("PlanToPrePlace", module, pose_key="pre_place_pose"), + actions.ExecuteTrajectory("ExecutePrePlace", module), + actions.PlanToPose("PlanToPlace", module, pose_key="place_pose"), + actions.ExecuteTrajectory("ExecutePlace", module), + actions.SetGripper( + "OpenGripper", + module, + position=cfg.bt_gripper_open_position, + settle_time=0.5, + ), + actions.PlanToPose("PlanRetract", module, pose_key="pre_place_pose"), + actions.ExecuteTrajectory("ExecuteRetract", module), + actions.SetResultMessage( + "SetResult", + module, + message=f"Place complete - object released at ({x:.3f}, {y:.3f}, {z:.3f})", + ), + ], + ) + + bb = py_trees.blackboard.Client(name="PlaceTreeSetup") + bb.register_key(key="robot_name", access=py_trees.common.Access.WRITE) + bb.robot_name = robot_name + return root diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 84ede61793..9edbe8bfca 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -33,6 +33,7 @@ from dimos.core.core import rpc from dimos.core.docker_runner import DockerModule as DockerRunner from dimos.core.stream import In # noqa: TC001 +from dimos.manipulation.bt.trees import build_pick_tree, build_place_tree from dimos.manipulation.grasping.graspgen_module import GraspGenModule from dimos.manipulation.manipulation_module import ( ManipulationModule, @@ -72,6 +73,32 @@ class PickAndPlaceModuleConfig(ManipulationModuleConfig): default_factory=lambda: Path.home() / ".dimos" / "graspgen" / "visualization.json" ) + # Behavior-tree orchestration settings + bt_enable_graspgen: bool = True + bt_tick_rate_hz: float = 20.0 + bt_max_pick_attempts: int = 5 + bt_max_rescan_attempts: int = 3 + bt_scan_duration: float = 1.0 + bt_execution_timeout: float | None = None + + # Gripper/lift verification settings + bt_gripper_open_position: float = 0.85 + bt_gripper_close_position: float = 0.0 + bt_gripper_grasp_threshold: float = 0.005 + bt_gripper_grasp_max_open_fraction: float = 0.5 + bt_gripper_grasp_max_open_position: float | None = None + bt_gripper_grasp_open_reference: float | None = None + bt_gripper_grasp_min_closure: float | None = None + bt_gripper_settle_time: float = 1.5 + bt_lift_height: float = 0.10 + + # Workspace filtering for generated grasps + bt_min_grasp_z: float = 0.05 + bt_max_grasp_distance: float = 0.9 + bt_max_approach_angle: float = 1.05 + bt_heuristic_grasp_z_offsets: tuple[float, ...] = (0.08, 0.12, 0.16) + bt_min_heuristic_grasp_z: float = 0.05 + class PickAndPlaceModule(ManipulationModule): """Manipulation module with perception integration and pick-and-place skills. @@ -84,6 +111,12 @@ class PickAndPlaceModule(ManipulationModule): default_config = PickAndPlaceModuleConfig + rpc_calls: list[str] = [ + "ObjectSceneRegistrationModule.get_object_pointcloud_by_name", + "ObjectSceneRegistrationModule.get_object_pointcloud_by_object_id", + "ObjectSceneRegistrationModule.get_full_scene_pointcloud", + ] + # Type annotation for the config attribute (mypy uses this) config: PickAndPlaceModuleConfig @@ -323,6 +356,74 @@ def _generate_grasps_for_pick( logger.info(f"Heuristic grasp for '{object_name}' at ({c.x:.3f}, {c.y:.3f}, {c.z:.3f})") return [grasp_pose] + @staticmethod + def _read_bt_blackboard(key: str, fallback: str) -> str: + """Read a py_trees blackboard key, returning fallback if unavailable.""" + import py_trees + + try: + bb = py_trees.blackboard.Client(name="PickPlaceBTReader") + bb.register_key(key=key, access=py_trees.common.Access.READ) + value = getattr(bb, key) + return str(value) if value else fallback + except (AttributeError, KeyError): + return fallback + + def _tick_bt_tree(self, root: Any) -> str: + """Tick a behavior tree until it succeeds, fails, or times out.""" + import py_trees + + bb = py_trees.blackboard.Client(name="PickPlaceBTReset") + defaults: dict[str, Any] = { + "detections": [], + "target_object": None, + "object_pointcloud": None, + "scene_pointcloud": None, + "grasp_candidates": [], + "grasp_index": 0, + "current_grasp": None, + "pre_grasp_pose": None, + "place_pose": None, + "pre_place_pose": None, + "lift_pose": None, + "has_object": False, + "grasp_source": "", + "error_message": "", + "result_message": "", + } + for key, value in defaults.items(): + bb.register_key(key=key, access=py_trees.common.Access.WRITE) + setattr(bb, key, value) + + tree = py_trees.trees.BehaviourTree(root=root) + tree.setup() + + start = time.time() + period = 1.0 / self.config.bt_tick_rate_hz + while True: + tree.tick() + + if root.status == py_trees.common.Status.SUCCESS: + return self._read_bt_blackboard( + "result_message", "Operation completed successfully" + ) + + if root.status == py_trees.common.Status.FAILURE: + return self._read_bt_blackboard("error_message", "Error: Operation failed") + + if ( + self.config.bt_execution_timeout is not None + and time.time() - start > self.config.bt_execution_timeout + ): + root.stop(py_trees.common.Status.INVALID) + try: + self.cancel() + except Exception: + logger.warning("Failed to cancel timed-out BT operation", exc_info=True) + return "Error: Operation timed out" + + time.sleep(period) + # ========================================================================= # Perception Skills # ========================================================================= @@ -433,65 +534,16 @@ def pick( object_id: Optional unique object ID from perception for precise identification. robot_name: Robot to use (only needed for multi-arm setups). """ - robot = self._get_robot(robot_name) - if robot is None: + if self._get_robot(robot_name) is None: return "Error: Robot not found" - rname, _, config, _ = robot - pre_grasp_offset = config.pre_grasp_offset - - # 1. Generate grasps (uses already-cached detections — call scan_objects first) - logger.info(f"Generating grasp poses for '{object_name}'...") - grasp_poses = self._generate_grasps_for_pick(object_name, object_id) - if not grasp_poses: - return f"Error: No grasp poses found for '{object_name}'. Object may not be detected." - - # 2. Try each grasp candidate - max_attempts = min(len(grasp_poses), 5) - for i, grasp_pose in enumerate(grasp_poses[:max_attempts]): - pre_grasp_pose = self._compute_pre_grasp_pose(grasp_pose, pre_grasp_offset) - - logger.info(f"Planning approach to pre-grasp (attempt {i + 1}/{max_attempts})...") - if not self.plan_to_pose(pre_grasp_pose, rname): - logger.info(f"Grasp candidate {i + 1} approach planning failed, trying next") - continue # Try next candidate - - # Open gripper before approach - logger.info("Opening gripper...") - self._set_gripper_position(0.85, rname) - time.sleep(0.5) - - # 3. Preview + execute approach - err = self._preview_execute_wait(rname) - if err: - return err - - # 4. Move to grasp pose - logger.info("Moving to grasp position...") - if not self.plan_to_pose(grasp_pose, rname): - return "Error: Grasp pose planning failed" - err = self._preview_execute_wait(rname) - if err: - return err - - # 5. Close gripper - logger.info("Closing gripper...") - self._set_gripper_position(0.0, rname) - time.sleep(1.5) # Wait for gripper to close - - # 6. Retract to pre-grasp - logger.info("Retracting with object...") - if not self.plan_to_pose(pre_grasp_pose, rname): - return "Error: Retract planning failed" - err = self._preview_execute_wait(rname) - if err: - return err - - # Store pick position so place_back() can return the object - self._last_pick_position = grasp_pose.position - - return f"Pick complete — grasped '{object_name}' successfully" - - return f"Error: All {max_attempts} grasp attempts failed for '{object_name}'" + logger.info(f"Starting BT pick for '{object_name}'") + root = build_pick_tree( + module=self, + object_name=object_name, + object_id=object_id, + robot_name=robot_name, + ) + return self._tick_bt_tree(root) @skill def place( @@ -512,47 +564,11 @@ def place( z: Target Z position in meters. robot_name: Robot to use (only needed for multi-arm setups). """ - robot = self._get_robot(robot_name) - if robot is None: + if self._get_robot(robot_name) is None: return "Error: Robot not found" - rname, _, config, _ = robot - pre_place_offset = config.pre_grasp_offset - - # Compute place pose (top-down approach) - place_pose = Pose(Vector3(x, y, z), Quaternion.from_euler(Vector3(0.0, math.pi, 0.0))) - pre_place_pose = self._compute_pre_grasp_pose(place_pose, pre_place_offset) - - # 1. Move to pre-place - logger.info(f"Planning approach to place position ({x:.3f}, {y:.3f}, {z:.3f})...") - if not self.plan_to_pose(pre_place_pose, rname): - return "Error: Pre-place approach planning failed" - - err = self._preview_execute_wait(rname) - if err: - return err - - # 2. Lower to place position - logger.info("Lowering to place position...") - if not self.plan_to_pose(place_pose, rname): - return "Error: Place pose planning failed" - err = self._preview_execute_wait(rname) - if err: - return err - - # 3. Release - logger.info("Releasing object...") - self._set_gripper_position(0.85, rname) - time.sleep(1.0) - - # 4. Retract - logger.info("Retracting...") - if not self.plan_to_pose(pre_place_pose, rname): - return "Error: Retract planning failed" - err = self._preview_execute_wait(rname) - if err: - return err - - return f"Place complete — object released at ({x:.3f}, {y:.3f}, {z:.3f})" + logger.info(f"Starting BT place at ({x:.3f}, {y:.3f}, {z:.3f})") + root = build_place_tree(module=self, x=x, y=y, z=z, robot_name=robot_name) + return self._tick_bt_tree(root) @skill def place_back(self, robot_name: str | None = None) -> str: diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 22fe731a70..1fadfd175c 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -99,6 +99,8 @@ def __getstate__(self) -> dict[str, object]: # Remove non-picklable objects del state["_pcd_tensor"] state["_pcd_legacy_cache"] = None + state.pop("axis_aligned_bounding_box", None) + state.pop("oriented_bounding_box", None) return state def __setstate__(self, state: dict[str, object]) -> None: diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index f48802ab7a..430f56be4a 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -14,6 +14,8 @@ # limitations under the License. +import pickle + import numpy as np from dimos.msgs.sensor_msgs import PointCloud2 @@ -153,3 +155,31 @@ def test_bounding_box_intersects() -> None: pass print("✓ All bounding box intersection tests passed!") + + +def test_pickle_after_cached_open3d_bounding_boxes() -> None: + """Pickle should drop cached Open3D bounding boxes and preserve points.""" + points = np.array( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + [1.0, 1.0, 0.0], + [1.0, 0.0, 1.0], + [0.0, 1.0, 1.0], + [1.0, 1.0, 1.0], + ], + dtype=np.float32, + ) + pc = PointCloud2.from_numpy(points, frame_id="world", timestamp=123.0) + + _ = pc.axis_aligned_bounding_box + _ = pc.oriented_bounding_box + + restored = pickle.loads(pickle.dumps(pc)) + restored_points, _ = restored.as_numpy() + + np.testing.assert_allclose(restored_points, points) + assert restored.frame_id == "world" + assert restored.ts == 123.0 diff --git a/dimos/protocol/pubsub/encoders.py b/dimos/protocol/pubsub/encoders.py index 6b2056fa8b..6ee32e37ad 100644 --- a/dimos/protocol/pubsub/encoders.py +++ b/dimos/protocol/pubsub/encoders.py @@ -113,7 +113,12 @@ def encode(self, msg: DimosMsg | bytes, _: LCMTopicProto) -> bytes: def decode(self, msg: bytes, topic: LCMTopicProto) -> DimosMsg: if topic.lcm_type is None: raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no lcm_type") - return topic.lcm_type.lcm_decode(msg) + try: + return topic.lcm_type.lcm_decode(msg) + except AttributeError as exc: + if getattr(topic.lcm_type, "msg_name", "") == "foxglove_msgs.ImageAnnotations": + raise DecodingError("Skipping undecodable Foxglove ImageAnnotations") from exc + raise class JpegEncoderMixin(PubSubEncoderMixin[LCMTopicProto, Image, bytes]): diff --git a/pyproject.toml b/pyproject.toml index 017562a78a..9b92f4a803 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -84,6 +84,8 @@ dependencies = [ "rerun-sdk>=0.20.0", "dimos-viewer>=0.30.0a2", "toolz>=1.1.0", + # Behavior Trees + "py-trees>=2.2,<3", "protobuf>=6.33.5,<7", "psutil>=7.0.0", ] diff --git a/uv.lock b/uv.lock index 5ec39fff59..30fd37f6d6 100644 --- a/uv.lock +++ b/uv.lock @@ -1697,6 +1697,7 @@ dependencies = [ { name = "plum-dispatch" }, { name = "protobuf" }, { name = "psutil" }, + { name = "py-trees" }, { name = "pydantic" }, { name = "pydantic-settings" }, { name = "python-dotenv" }, @@ -2060,6 +2061,7 @@ requires-dist = [ { name = "psutil", specifier = ">=7.0.0" }, { name = "psycopg2-binary", marker = "extra == 'psql'", specifier = ">=2.9.11" }, { name = "py-spy", marker = "extra == 'dev'" }, + { name = "py-trees", specifier = ">=2.2,<3" }, { name = "pydantic" }, { name = "pydantic", marker = "extra == 'docker'" }, { name = "pydantic-settings", specifier = ">=2.11.0,<3" }, @@ -7040,6 +7042,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/e1/da/fcc9a9fcd4ca946ff402cff20348e838b051d69f50f5d1f5dca4cd3c5eb8/py_spy-0.4.1-py2.py3-none-win_amd64.whl", hash = "sha256:d92e522bd40e9bf7d87c204033ce5bb5c828fca45fa28d970f58d71128069fdc", size = 1818784, upload-time = "2025-07-31T19:33:23.802Z" }, ] +[[package]] +name = "py-trees" +version = "2.4.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "pydot" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/79/9c/b8c39a86ce887e3547b415e6bd61b70e1004af0a8ba1b44e75233a4406a2/py_trees-2.4.0.tar.gz", hash = "sha256:cd8738a80e7422aec2b133270b6aa92e3e83926ded2ba9b3614028dba99cdb41", size = 85359, upload-time = "2025-11-13T22:46:01.41Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/7d/0a/2e0c30342586dabb0b4b7946393a7aeb725fca7b4549fb02e128a1a0fe87/py_trees-2.4.0-py3-none-any.whl", hash = "sha256:c26a2d23f70871ce093abd5f8425998529bfe7b97cc563316e9c3c9cec20c312", size = 114099, upload-time = "2025-11-13T22:45:58.884Z" }, +] + [[package]] name = "pyarrow" version = "23.0.0" @@ -7503,7 +7517,7 @@ name = "pydot" version = "4.0.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyparsing", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "pyparsing" }, ] sdist = { url = "https://files.pythonhosted.org/packages/50/35/b17cb89ff865484c6a20ef46bf9d95a5f07328292578de0b295f4a6beec2/pydot-4.0.1.tar.gz", hash = "sha256:c2148f681c4a33e08bf0e26a9e5f8e4099a82e0e2a068098f32ce86577364ad5", size = 162594, upload-time = "2025-06-17T20:09:56.454Z" } wheels = [