From 31d196c847c9bc0dbc6c9322c086e6a7f9a8d0c8 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 28 Sep 2026 21:34:12 +0000 Subject: [PATCH 1/5] Add Intrinsic MoveIt grasp planning Foxglove demo (containerized) Co-authored-by: Mateusz Sadowski --- README.md | 2 + .../ros2/intrinsic_moveit/.dockerignore | 4 + integrations/ros2/intrinsic_moveit/.gitignore | 2 + integrations/ros2/intrinsic_moveit/Dockerfile | 62 + integrations/ros2/intrinsic_moveit/README.md | 219 ++++ .../ros2/intrinsic_moveit/compose.yaml | 24 + .../intrinsic_moveit_grasp_demo.json | 652 ++++++++++ .../ros2/intrinsic_moveit/recordings/.gitkeep | 0 .../config/demo_params.yaml | 40 + .../intrinsic_foxglove_demo/__init__.py | 0 .../intrinsic_foxglove_demo/geometry.py | 143 +++ .../grasp_demo_driver.py | 1097 +++++++++++++++++ .../intrinsic_foxglove_demo/markers.py | 198 +++ .../launch/demo.launch.py | 106 ++ .../src/intrinsic_foxglove_demo/package.xml | 31 + .../resource/intrinsic_foxglove_demo | 0 .../src/intrinsic_foxglove_demo/setup.cfg | 4 + .../src/intrinsic_foxglove_demo/setup.py | 29 + .../CMakeLists.txt | 52 + .../package.xml | 33 + .../src/moveit_planning_node_standalone.cpp | 133 ++ .../scripts/check_foxglove_ws.py | 95 ++ .../intrinsic_moveit/scripts/check_motion.py | 92 ++ .../scripts/check_plan_grasps.py | 101 ++ .../intrinsic_moveit/scripts/entrypoint.sh | 58 + .../ros2/intrinsic_moveit/scripts/ros_env.py | 23 + 26 files changed, 3200 insertions(+) create mode 100644 integrations/ros2/intrinsic_moveit/.dockerignore create mode 100644 integrations/ros2/intrinsic_moveit/.gitignore create mode 100644 integrations/ros2/intrinsic_moveit/Dockerfile create mode 100644 integrations/ros2/intrinsic_moveit/README.md create mode 100644 integrations/ros2/intrinsic_moveit/compose.yaml create mode 100644 integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo.json create mode 100644 integrations/ros2/intrinsic_moveit/recordings/.gitkeep create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__init__.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/geometry.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/markers.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/launch/demo.launch.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/package.xml create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/resource/intrinsic_foxglove_demo create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.cfg create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.py create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/CMakeLists.txt create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/package.xml create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp create mode 100755 integrations/ros2/intrinsic_moveit/scripts/check_foxglove_ws.py create mode 100755 integrations/ros2/intrinsic_moveit/scripts/check_motion.py create mode 100755 integrations/ros2/intrinsic_moveit/scripts/check_plan_grasps.py create mode 100755 integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh create mode 100755 integrations/ros2/intrinsic_moveit/scripts/ros_env.py diff --git a/README.md b/README.md index d086a04..4215783 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,8 @@ Below is a list of all tutorials available in this repository: ### [ROS 2 Diagnostics Tutorial](integrations/ros2/diagnostics/README.md) - 📝 Basic example of publishing and visualizing DiagnosticArray messages - 🔗 [Related Blog Post](https://foxglove.dev/blog/a-practical-guide-to-using-ros-diagnostics) +### [Intrinsic MoveIt grasp planning (UR5e + Robotiq Hand-E) in Foxglove](integrations/ros2/intrinsic_moveit/README.md) +- 📝 Run Intrinsic's open-source MoveIt Task Constructor grasp planner for the OMTS cell on mock hardware in Docker and visualize candidates and motions live in Foxglove ### [ROS 2 Launch Files Tutorial](integrations/ros2/launch/README.md) - 📝 Code reference for ROS 2 launch files tutorial - 🔗 [Related Blog Post](https://foxglove.dev/blog/how-to-use-ros2-launch-files) diff --git a/integrations/ros2/intrinsic_moveit/.dockerignore b/integrations/ros2/intrinsic_moveit/.dockerignore new file mode 100644 index 0000000..b161246 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/.dockerignore @@ -0,0 +1,4 @@ +recordings/ +media/ +*.md +**/__pycache__ diff --git a/integrations/ros2/intrinsic_moveit/.gitignore b/integrations/ros2/intrinsic_moveit/.gitignore new file mode 100644 index 0000000..4c30283 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/.gitignore @@ -0,0 +1,2 @@ +recordings/* +!recordings/.gitkeep diff --git a/integrations/ros2/intrinsic_moveit/Dockerfile b/integrations/ros2/intrinsic_moveit/Dockerfile new file mode 100644 index 0000000..2376c5c --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/Dockerfile @@ -0,0 +1,62 @@ +# syntax=docker/dockerfile:1 +ARG ROS_DISTRO=jazzy +FROM ros:${ROS_DISTRO}-ros-base +ARG ROS_DISTRO +ARG INTRINSIC_MOVEIT_REPO=https://github.com/intrinsic-ai/intrinsic-moveit.git +ARG INTRINSIC_MOVEIT_COMMIT=c5e3290aa0f0e64c2d106a2fb4eb10cb52592205 +ENV DEBIAN_FRONTEND=noninteractive \ + ROS_HOME=/tmp/ros \ + ROS_LOG_DIR=/tmp/ros/log \ + INTRINSIC_MOVEIT_COMMIT=${INTRINSIC_MOVEIT_COMMIT} \ + ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST \ + RCUTILS_COLORIZED_OUTPUT=1 + +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + python3-numpy \ + python3-websockets \ + ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-moveit-task-constructor-core \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-ur-description \ + ros-${ROS_DISTRO}-foxglove-bridge \ + ros-${ROS_DISTRO}-rosbag2-storage-mcap \ + && rm -rf /var/lib/apt/lists/* + +RUN git clone ${INTRINSIC_MOVEIT_REPO} /opt/intrinsic-moveit \ + && git -C /opt/intrinsic-moveit checkout ${INTRINSIC_MOVEIT_COMMIT} \ + && rm -rf /opt/intrinsic-moveit/.git + +WORKDIR /ws +RUN mkdir -p src \ + && cp -r /opt/intrinsic-moveit/robot_hardware_description \ + /opt/intrinsic-moveit/third_party/robot_hardware_moveit_config \ + /opt/intrinsic-moveit/moveit_planning_interfaces src/ +COPY ros_ws/src/moveit_planning_service_standalone/ src/moveit_planning_service_standalone/ +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build --merge-install --parallel-workers 2 \ + --packages-up-to moveit_planning_service \ + --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF \ + -DINTRINSIC_MOVEIT_DIR=/opt/intrinsic-moveit \ + && rm -rf build log + +COPY ros_ws/src/intrinsic_foxglove_demo/ src/intrinsic_foxglove_demo/ +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && . /ws/install/setup.sh \ + && colcon build --merge-install --parallel-workers 2 \ + --packages-select intrinsic_foxglove_demo \ + --cmake-args -DCMAKE_BUILD_TYPE=Release \ + && rm -rf build log + +COPY scripts/ /opt/demo/scripts/ +COPY foxglove_layouts/ /opt/demo/foxglove_layouts/ +RUN chmod +x /opt/demo/scripts/*.sh \ + && printf 'source /opt/ros/%s/setup.bash\nsource /ws/install/setup.bash\n' "${ROS_DISTRO}" \ + > /etc/profile.d/ros_ws.sh \ + && printf '\n[ -f /opt/ros/%s/setup.bash ] && . /opt/ros/%s/setup.bash\n[ -f /ws/install/setup.bash ] && . /ws/install/setup.bash\n' \ + "${ROS_DISTRO}" "${ROS_DISTRO}" >> /etc/bash.bashrc + +EXPOSE 8765 +ENTRYPOINT ["/opt/demo/scripts/entrypoint.sh"] +CMD ["ros2", "launch", "intrinsic_foxglove_demo", "demo.launch.py"] diff --git a/integrations/ros2/intrinsic_moveit/README.md b/integrations/ros2/intrinsic_moveit/README.md new file mode 100644 index 0000000..f48a797 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/README.md @@ -0,0 +1,219 @@ +--- +title: "Intrinsic MoveIt grasp planning (UR5e + Robotiq Hand-E) in Foxglove" +short_description: "Run Intrinsic's open-source MoveIt Task Constructor grasp planner for the OMTS cell on mock hardware in Docker and visualize candidates and motions live in Foxglove" +--- + +# Intrinsic MoveIt grasp planning in Foxglove + +This tutorial runs [Intrinsic's open-source MoveIt grasp planner](https://github.com/intrinsic-ai/intrinsic-moveit) for the [Open Machine Tending Solution (OMTS)](https://github.com/intrinsic-ai/intrinsic-omts) cell and shows the result live in Foxglove. The cell is a UR5e, a Robotiq Hand-E, and an Orbbec Gemini 335Le wrist camera. Hardware is mocked with `ros2_control`, so the container needs no robot, GPU, or Intrinsic cluster. + +The grasp generator is Intrinsic's MoveIt Task Constructor pipeline (`CurrentState`, open the hand, `Connect`, a `MoveRelative` approach, allow-collision, and `GenerateBoxGraspPoses` inside `ComputeIK`). It is served at `/grasp_planning/plan_grasps`. A small Python driver plays the role that Intrinsic's Flowstate `moveit_plan_grasp_skill` plays in the full stack: it ranks candidates, asks MoveIt for trajectories, and drives the mock arm through pick and place. Every cycle drops the billet at a new pose inside the OMTS return-shift window and plans again. + +Upstream sources are pinned to [`c5e3290aa0f0e64c2d106a2fb4eb10cb52592205`](https://github.com/intrinsic-ai/intrinsic-moveit/commit/c5e3290aa0f0e64c2d106a2fb4eb10cb52592205) and compiled inside the image. They are not vendored into this repository. + +## What comes from upstream + +| Upstream path | In this demo | +| --- | --- | +| `robot_hardware_description` | Copied unmodified | +| `third_party/robot_hardware_moveit_config` | Copied unmodified | +| `moveit_planning_interfaces` (`PlanGrasps.srv`) | Copied unmodified | +| `grasp_planning_pipeline.cpp`, `generate_box_grasp_poses.cpp`, `object_geometry.cpp` and their headers | Compiled verbatim | +| `moveit_planning_service/launch/service.launch.py` | Installed and launched verbatim with mock hardware | +| `moveit_planning_node.cpp` | Replaced by an SDK-free standalone node with the same mock-mode services | +| Scene synchronizer, status monitor, Flowstate skills | Not built. They depend on the Intrinsic SDK | + +`/motion_planning/get_motion_plan` on the planning node is an upstream stub: it returns success and an empty trajectory. Trajectories in this demo come from MoveIt's `/plan_kinematic_path`, which uses the same `moveit_msgs/srv/GetMotionPlan` type. `PlanGrasps` itself returns grasp poses and IK solutions, not trajectories. + +## Architecture + +```mermaid +flowchart LR + subgraph container [Docker container] + launch["service.launch.py"] + rsp[robot_state_publisher] + mg[move_group] + ctrl["ros2_control mock + JTC + gripper"] + mps["moveit_planning_node (standalone)"] + drv[grasp_demo_driver] + bridge[foxglove_bridge :8765] + bag[rosbag2 MCAP] + launch --> rsp + launch --> mg + launch --> ctrl + launch --> mps + drv --> mps + drv --> mg + drv --> ctrl + bridge --> rsp + bridge --> drv + bag --> drv + end + app[Foxglove app] -->|ws://localhost:8765| bridge +``` + +## Requirements + +- Docker with Compose v2 +- About 6 GB of free disk (the image is about 3 GB) +- Tested on x86_64. Jazzy publishes arm64 builds of these packages, but that path is untested +- [Foxglove](https://foxglove.dev) desktop or [app.foxglove.dev](https://app.foxglove.dev) + +No display server or GPU is required. The container launches MoveIt with `headless:=true`. + +Packages used to build the image on Ubuntu Noble (versions float with the Jazzy apt snapshot; recorded September 2026): + +| Package | Version observed in this image | +| --- | --- | +| `ros-jazzy-moveit` | 2.12.4-1noble.20260905.083030 | +| `ros-jazzy-moveit-task-constructor-core` | 0.1.8-1noble.20260904.024044 | +| `ros-jazzy-foxglove-bridge` | 3.5.0-1noble.20260902.084741 | +| `ros-jazzy-rosbag2-storage-mcap` | 0.26.11-1noble.20260903.070458 | +| `ros-jazzy-ur-description` | 3.5.1-1noble.20260905.072414 | + +## Quick start + +```bash +cd integrations/ros2/intrinsic_moveit +docker compose up --build +``` + +The first build downloads MoveIt and compiles the standalone planning node. Later starts reuse the image. + +Then in Foxglove: + +1. Open a connection, choose **Foxglove WebSocket**, and enter `ws://localhost:8765`. +2. Import [`foxglove_layouts/intrinsic_moveit_grasp_demo.json`](foxglove_layouts/intrinsic_moveit_grasp_demo.json). + +Stop the stack with Ctrl-C or: + +```bash +docker compose down +``` + +SIGINT lets rosbag2 finish the MCAP summary. Recordings land in `./recordings/`. + +Headless checks (no Foxglove app): + +```bash +docker compose exec intrinsic-moveit-demo python3 /opt/demo/scripts/check_foxglove_ws.py +docker compose exec intrinsic-moveit-demo python3 /opt/demo/scripts/check_plan_grasps.py +docker compose exec intrinsic-moveit-demo python3 /opt/demo/scripts/check_motion.py +``` + +## What you are seeing + +The arm stands on a grey table. The shaded rectangle is the OMTS return-shift window (center `(0.45, 0)`, ±0.1 m, ±40°). An aluminium-colored `raw_stock_2x3x5` billet (`0.0762 x 0.127 x 0.0508` m, standing on its 3 inch edge) appears in that window. + +| Phase | What happens | +| --- | --- | +| `SPAWN_WORKPIECE` | The billet is added to the MoveIt scene | +| `PLAN_GRASPS` | `/grasp_planning/plan_grasps` runs the MTC pipeline. Distinct candidates are drawn on the billet | +| `SELECT_GRASP` | The best feasible candidate turns green. The Raw Messages panel shows the `moveit_msgs/Grasp` | +| `OPEN_GRIPPER` | The Hand-E opens | +| `MOVE_TO_PREGRASP` | OMPL plans to Intrinsic's pre-grasp IK solution and the arm moves | +| `APPROACH` | A 10 cm Cartesian move along the tool to the grasp pose | +| `GRASP` | The fingers close to the billet width and the object is attached to `hande_tcp` | +| `RETREAT` | 10 cm back along the tool | +| `PLACE` | A translucent ghost shows the next pose in the return-shift window. The arm transits there | +| `PLACE_DESCEND` | Cartesian move down onto the ghost | +| `RELEASE` | The gripper opens and the billet is detached | +| `RETREAT_UP` | The tool backs off | +| `PARK` | The gripper closes and the arm returns to the SRDF `ready` pose. The cycle counter increments | + +The loop then plans a new grasp for the billet at its new pose. Candidates are ranked by `grasp_quality` (highest first), with a more top-down approach winning ties. Up to three distinct poses are tried if a pre-grasp motion fails. + +The billet is 50.8 mm across the gripped face and the Hand-E stroke is about 50 mm (`open = -0.001`, `closed = 0.025` on `hande_left_finger_joint`). The fingers barely move at the moment of grasp. The hand is opened before the approach and parked closed between cycles so the motion is visible. + +### Topics + +| Topic | Type | Contents | +| --- | --- | --- | +| `/demo/scene_markers` | `visualization_msgs/MarkerArray` | Table, return-shift window, billet, place ghost | +| `/demo/grasp_candidates` | `visualization_msgs/MarkerArray` | Gripper glyphs, approach arrows, quality labels | +| `/demo/grasp_poses` | `geometry_msgs/PoseArray` | Every grasp pose, in the `raw_stock` frame | +| `/demo/pregrasp_poses` | `geometry_msgs/PoseArray` | Matching pre-grasp poses | +| `/demo/selected_grasp` | `geometry_msgs/PoseStamped` | Chosen grasp pose | +| `/demo/selected_grasp_msg` | `moveit_msgs/Grasp` | Raw message returned by Intrinsic's planner | +| `/demo/planned_tcp_path` | `nav_msgs/Path` | TCP samples of the last trajectory | +| `/demo/status` | `std_msgs/String` | Current phase | +| `/demo/cycle`, `/demo/failures` | `std_msgs/Int32` | Completed cycles and recoveries | +| `/demo/grasp_planning/num_candidates` | `std_msgs/Int32` | Candidates from the last `PlanGrasps` call | +| `/demo/grasp_planning/latency_s` | `std_msgs/Float64` | Wall time of that call | +| `/robot_description_web` | `std_msgs/String` | URDF with `package://` mesh URIs rewritten to the pinned GitHub commit | + +Also published by the upstream launch: `/robot_description`, `/robot_description_semantic`, `/tf`, `/tf_static`, `/joint_states`, `/ur_manipulator_controller/controller_state`, and `/rosout`. + +The layout's **Details** tab plots commanded and actual arm joints from the joint trajectory controller, the gripper joint (`/joint_states.position[1]`, `hande_left_finger_joint`), and grasp-planning latency. + +## Recordings + +With `RECORD=true` (the default), rosbag2 writes an MCAP file under `./recordings/intrinsic_grasp_demo_/`. Open that file in Foxglove, import the same layout, and enable the **URDF (offline web)** layer (topic `/robot_description_web`) while hiding the live `/robot_description` layer. Mesh URLs then load from `raw.githubusercontent.com` at the pinned commit, which sends `access-control-allow-origin: *`. + +Live Foxglove sessions should keep the `/robot_description` URDF layer enabled. The bridge fetches `package://` meshes itself. The Hand-E body DAE is about 12 MB, so the first load is slow. + +## Call the grasp service yourself + +```bash +docker compose exec -it intrinsic-moveit-demo bash +``` + +The interactive shell sources the workspace. The demo keeps a `raw_stock` collision object in the scene: + +```bash +ros2 service call /grasp_planning/plan_grasps moveit_planning_interfaces/srv/PlanGrasps "{ + group_name: ur_manipulator, + end_effector_group: hand, + tool_frame: hande_tcp, + planning_timeout_sec: 10.0, + gripper_motion_duration_sec: 0.75, + retract_dist_m: 0.1, + surfaces: [0, 1, 2, 3, 4, 5], + num_rotations: 4, + target: {id: raw_stock} +}" +``` + +## Differences from the upstream deployment + +- The image does not build the Intrinsic SDK, Flowstate bridge, Zenoh pubsub, or status monitor. +- Only mock hardware is supported. `use_mock_hardware:=false` logs a warning and still runs the mock path. +- World objects are inserted by the demo driver through `/apply_planning_scene`, not synchronized from Flowstate. +- Arm and Cartesian trajectories come from `move_group` (`/plan_kinematic_path`, `/compute_cartesian_path`, `/execute_trajectory`). The planning node's `/motion_planning/get_motion_plan` service is left as the upstream empty-trajectory stub. + +## Configuration + +| Control | Where | Default | +| --- | --- | --- | +| `RECORD` | container environment | `true` | +| `DEMO_CYCLES` | container environment | `0` (run until stopped). A positive value stops after that many cycles and leaves the bridge up | +| `INTRINSIC_MOVEIT_COMMIT` | Docker build arg | `c5e3290aa0f0e64c2d106a2fb4eb10cb52592205` | +| Scene, speeds, seed | `ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml` | OMTS billet and return-shift window, seed `7` | + +Example of a finite run: + +```bash +DEMO_CYCLES=5 docker compose up +``` + +`DEMO_CYCLES` is read by `scripts/entrypoint.sh` and passed as the `cycles:=` launch argument. The seeded RNG makes place poses repeatable for a given seed. + +To move the pin, set the build arg and rebuild: + +```bash +docker compose build --build-arg INTRINSIC_MOVEIT_COMMIT= +``` + +The standalone node only compiles the SDK-free sources. A commit that changes those files' APIs will need a matching driver update. + +## Troubleshooting + +- **Port 8765 is in use.** Stop the other process or change the host mapping in `compose.yaml`. +- **The robot has no meshes.** Wait for the first asset fetch (tens of megabytes). Confirm the bridge is the Foxglove WebSocket endpoint, not a raw rosbridge URL. The subprotocol is `foxglove.sdk.v1`. +- **Offline playback has no meshes.** Toggle the URDF layer to `/robot_description_web`. +- **The arm pauses in `RECOVER`.** A sampled place pose was unreachable. The driver detaches, returns to ready, and samples a new billet pose. `/demo/failures` counts these events. +- **Logs.** `docker compose logs -f`. + +## License + +`intrinsic-moveit` is Apache-2.0. `robot_hardware_moveit_config` is BSD-3-Clause. The standalone `moveit_planning_node` is derived from Intrinsic's `moveit_planning_node.cpp` and stays under Apache-2.0. The demo driver, launch file, and layout in this folder are Apache-2.0. diff --git a/integrations/ros2/intrinsic_moveit/compose.yaml b/integrations/ros2/intrinsic_moveit/compose.yaml new file mode 100644 index 0000000..2e380c0 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/compose.yaml @@ -0,0 +1,24 @@ +services: + intrinsic-moveit-demo: + build: + context: . + args: + INTRINSIC_MOVEIT_COMMIT: c5e3290aa0f0e64c2d106a2fb4eb10cb52592205 + image: foxglove-tutorials/intrinsic-moveit-demo:latest + ports: + - "8765:8765" + volumes: + - ./recordings:/recordings + environment: + RECORD: "${RECORD:-true}" + DEMO_CYCLES: "${DEMO_CYCLES:-0}" + shm_size: 1gb + init: false + stop_signal: SIGINT + stop_grace_period: 30s + healthcheck: + test: ["CMD-SHELL", "bash -c ' 0.0: + scale = math.sqrt(trace + 1.0) * 2.0 + w = 0.25 * scale + x = (r[2, 1] - r[1, 2]) / scale + y = (r[0, 2] - r[2, 0]) / scale + z = (r[1, 0] - r[0, 1]) / scale + elif r[0, 0] > r[1, 1] and r[0, 0] > r[2, 2]: + scale = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0 + w = (r[2, 1] - r[1, 2]) / scale + x = 0.25 * scale + y = (r[0, 1] + r[1, 0]) / scale + z = (r[0, 2] + r[2, 0]) / scale + elif r[1, 1] > r[2, 2]: + scale = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0 + w = (r[0, 2] - r[2, 0]) / scale + x = (r[0, 1] + r[1, 0]) / scale + y = 0.25 * scale + z = (r[1, 2] + r[2, 1]) / scale + else: + scale = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0 + w = (r[1, 0] - r[0, 1]) / scale + x = (r[0, 2] + r[2, 0]) / scale + y = (r[1, 2] + r[2, 1]) / scale + z = 0.25 * scale + return (float(x), float(y), float(z), float(w)) + + +def translation(x, y, z): + transform = np.eye(4) + transform[:3, 3] = (float(x), float(y), float(z)) + return transform + + +def rotation_z(yaw): + cosine = math.cos(float(yaw)) + sine = math.sin(float(yaw)) + transform = np.eye(4) + transform[0, 0] = cosine + transform[0, 1] = -sine + transform[1, 0] = sine + transform[1, 1] = cosine + return transform + + +def pose_to_matrix(pose): + transform = np.eye(4) + transform[:3, :3] = quat_to_mat(( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + )) + transform[:3, 3] = (pose.position.x, pose.position.y, pose.position.z) + return transform + + +def matrix_from_xyz_quat(xyz, xyzw): + transform = np.eye(4) + transform[:3, :3] = quat_to_mat(xyzw) + transform[:3, 3] = np.asarray(xyz, dtype=float) + return transform + + +def matrix_to_pose(transform): + from geometry_msgs.msg import Pose + pose = Pose() + pose.position.x = float(transform[0, 3]) + pose.position.y = float(transform[1, 3]) + pose.position.z = float(transform[2, 3]) + x, y, z, w = mat_to_quat(transform[:3, :3]) + pose.orientation.x = x + pose.orientation.y = y + pose.orientation.z = z + pose.orientation.w = w + return pose + + +def matrix_to_transform(transform): + from geometry_msgs.msg import Transform + out = Transform() + out.translation.x = float(transform[0, 3]) + out.translation.y = float(transform[1, 3]) + out.translation.z = float(transform[2, 3]) + x, y, z, w = mat_to_quat(transform[:3, :3]) + out.rotation.x = x + out.rotation.y = y + out.rotation.z = z + out.rotation.w = w + return out + + +def compose(first, second): + return first @ second + + +def invert(transform): + rotation = transform[:3, :3] + translation_vec = transform[:3, 3] + out = np.eye(4) + out[:3, :3] = rotation.T + out[:3, 3] = -rotation.T @ translation_vec + return out + + +def pose_key(pose, pos_decimals=3, quat_decimals=3): + quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + if quat[3] < 0.0 or (quat[3] == 0.0 and quat[0] < 0.0): + quat = [-value for value in quat] + return ( + round(pose.position.x, pos_decimals), + round(pose.position.y, pos_decimals), + round(pose.position.z, pos_decimals), + round(quat[0], quat_decimals), + round(quat[1], quat_decimals), + round(quat[2], quat_decimals), + round(quat[3], quat_decimals), + ) + + +def vertical_half_extent(rotation, dims): + return 0.5 * sum(abs(float(rotation[2, index])) * float(dims[index]) for index in range(3)) diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py new file mode 100644 index 0000000..de792e1 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py @@ -0,0 +1,1097 @@ +import math +import threading +import time +import traceback + +import numpy as np +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory, GripperCommand +from geometry_msgs.msg import PoseArray, PoseStamped, TransformStamped +from moveit_msgs.action import ExecuteTrajectory +from moveit_msgs.msg import ( + AttachedCollisionObject, + CollisionObject, + Constraints, + Grasp, + JointConstraint, + MoveItErrorCodes, + PlanningScene, + PlanningSceneComponents, + RobotState, + RobotTrajectory, +) +from moveit_msgs.srv import ( + ApplyPlanningScene, + GetCartesianPath, + GetMotionPlan, + GetPlanningScene, + GetPositionFK, + GetPositionIK, +) +from moveit_planning_interfaces.srv import PlanGrasps +from nav_msgs.msg import Path +from rclpy.action import ActionClient +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import JointState +from shape_msgs.msg import SolidPrimitive +from std_msgs.msg import Float64, Int32, String +from tf2_ros import TransformBroadcaster +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from visualization_msgs.msg import MarkerArray + +from intrinsic_foxglove_demo.geometry import ( + invert, + matrix_from_xyz_quat, + matrix_to_pose, + matrix_to_transform, + pose_key, + pose_to_matrix, + quat_to_mat, + rotation_z, + translation, + vertical_half_extent, +) +from intrinsic_foxglove_demo.markers import build_candidate_markers, build_scene_markers + +PINNED_COMMIT = 'c5e3290aa0f0e64c2d106a2fb4eb10cb52592205' +ARM_JOINTS = [ + 'shoulder_pan_joint', + 'shoulder_lift_joint', + 'elbow_joint', + 'wrist_1_joint', + 'wrist_2_joint', + 'wrist_3_joint', +] +HOME_JOINTS = { + 'shoulder_pan_joint': 0.0, + 'shoulder_lift_joint': -1.5708, + 'elbow_joint': -1.5708, + 'wrist_1_joint': -1.5708, + 'wrist_2_joint': 1.5708, + 'wrist_3_joint': 0.0, +} +TOUCH_LINKS = [ + 'hande_hande_base_link', + 'hande_hande_finger_link_l', + 'hande_hande_finger_link_r', +] + + +def _duration(seconds): + duration = Duration() + duration.sec = int(seconds) + duration.nanosec = int(round((seconds - duration.sec) * 1e9)) + return duration + + +class GraspDemoDriver(Node): + def __init__(self): + super().__init__('grasp_demo_driver') + self._cb = ReentrantCallbackGroup() + self._lock = threading.Lock() + self._joints = {} + self._have_joints = threading.Event() + self._attached = False + self._tf_enabled = False + self._T_world_obj = np.eye(4) + self._T_tcp_obj = np.eye(4) + self._pending_pose = None + self._ghost_pose = None + self._displays = [] + self._tries = [] + self._selected = None + self.cycle = 0 + self.failures = 0 + + self._declare_parameters() + self._load_parameters() + + latched = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.scene_pub = self.create_publisher(MarkerArray, '/demo/scene_markers', latched) + self.candidate_pub = self.create_publisher( + MarkerArray, '/demo/grasp_candidates', latched) + self.path_pub = self.create_publisher(Path, '/demo/planned_tcp_path', latched) + self.status_pub = self.create_publisher(String, '/demo/status', latched) + self.web_urdf_pub = self.create_publisher(String, '/robot_description_web', latched) + self.grasp_poses_pub = self.create_publisher(PoseArray, '/demo/grasp_poses', 10) + self.pregrasp_poses_pub = self.create_publisher(PoseArray, '/demo/pregrasp_poses', 10) + self.selected_pose_pub = self.create_publisher(PoseStamped, '/demo/selected_grasp', 10) + self.selected_msg_pub = self.create_publisher(Grasp, '/demo/selected_grasp_msg', 10) + self.cycle_pub = self.create_publisher(Int32, '/demo/cycle', 10) + self.failures_pub = self.create_publisher(Int32, '/demo/failures', 10) + self.count_pub = self.create_publisher(Int32, '/demo/grasp_planning/num_candidates', 10) + self.latency_pub = self.create_publisher(Float64, '/demo/grasp_planning/latency_s', 10) + + self.create_subscription( + JointState, '/joint_states', self._on_joint_states, 10, callback_group=self._cb) + self.create_subscription( + String, '/robot_description', self._on_robot_description, latched, + callback_group=self._cb) + + self.tf_broadcaster = TransformBroadcaster(self) + self.create_timer(1.0 / 30.0, self._publish_tf, callback_group=self._cb) + self.create_timer(1.0, self._publish_scene, callback_group=self._cb) + + self.apply_scene = self.create_client( + ApplyPlanningScene, '/apply_planning_scene', callback_group=self._cb) + self.get_scene = self.create_client( + GetPlanningScene, '/get_planning_scene', callback_group=self._cb) + self.plan_grasps = self.create_client( + PlanGrasps, '/grasp_planning/plan_grasps', callback_group=self._cb) + self.motion_plan = self.create_client( + GetMotionPlan, self.motion_plan_service, callback_group=self._cb) + self.cartesian = self.create_client( + GetCartesianPath, '/compute_cartesian_path', callback_group=self._cb) + self.ik = self.create_client(GetPositionIK, '/compute_ik', callback_group=self._cb) + self.fk = self.create_client(GetPositionFK, '/compute_fk', callback_group=self._cb) + self.execute = ActionClient( + self, ExecuteTrajectory, '/execute_trajectory', callback_group=self._cb) + self.gripper = ActionClient( + self, GripperCommand, '/hand_controller/gripper_cmd', callback_group=self._cb) + self.follow_joints = ActionClient( + self, FollowJointTrajectory, '/ur_manipulator_controller/follow_joint_trajectory', + callback_group=self._cb) + + self._publish_counters() + self._publish_scene() + + def _declare_parameters(self): + self.declare_parameter('object_id', 'raw_stock') + self.declare_parameter('object_label', 'raw_stock_2x3x5') + self.declare_parameter('object_dims', [0.0762, 0.127, 0.0508]) + self.declare_parameter('object_base_quat_xyzw', [0.70710678, 0.0, 0.70710678, 0.0]) + self.declare_parameter('table_size', [1.2, 1.2, 0.04]) + self.declare_parameter('table_center', [0.3, 0.0, -0.021]) + self.declare_parameter('return_shift_center', [0.45, 0.0]) + self.declare_parameter('return_shift_bounds_xy', [0.1, 0.1]) + self.declare_parameter('return_shift_bounds_yaw_deg', 40.0) + self.declare_parameter('initial_object_xy_yaw_deg', [0.45, 0.1, 30.0]) + self.declare_parameter('min_place_distance', 0.06) + self.declare_parameter('random_seed', 7) + self.declare_parameter('group_name', 'ur_manipulator') + self.declare_parameter('end_effector_group', 'hand') + self.declare_parameter('tool_frame', 'hande_tcp') + self.declare_parameter('planning_timeout_sec', 10.0) + self.declare_parameter('gripper_motion_duration_sec', 0.75) + self.declare_parameter('retract_dist_m', 0.1) + self.declare_parameter('surfaces', [0, 1, 2, 3, 4, 5]) + self.declare_parameter('num_rotations', 4) + defaults = { + 'shoulder_pan_joint': -0.1597, + 'shoulder_lift_joint': -1.3542, + 'elbow_joint': -1.6648, + 'wrist_1_joint': -1.6933, + 'wrist_2_joint': 1.571, + 'wrist_3_joint': 1.411, + } + for name, value in defaults.items(): + self.declare_parameter(f'ready_joints.{name}', value) + self.declare_parameter('transit_velocity_scaling', 0.5) + self.declare_parameter('cartesian_velocity_scaling', 0.2) + self.declare_parameter('gripper_open', -0.001) + self.declare_parameter('gripper_closed', 0.025) + self.declare_parameter('gripper_nominal_stroke', 0.050) + self.declare_parameter('gripper_max_opening', 0.052) + self.declare_parameter('cycles', 0) + self.declare_parameter('pause_between_phases_sec', 0.4) + self.declare_parameter('motion_plan_service', '/plan_kinematic_path') + self.declare_parameter( + 'robot_description_web_base', + 'https://raw.githubusercontent.com/intrinsic-ai/intrinsic-moveit/{commit}/robot_hardware_description/') + self.declare_parameter('intrinsic_moveit_commit', '') + + def _load_parameters(self): + def doubles(name): + return [float(value) for value in self.get_parameter(name).value] + + self.object_id = self.get_parameter('object_id').value + self.object_label = self.get_parameter('object_label').value + self.object_dims = doubles('object_dims') + quat = doubles('object_base_quat_xyzw') + self.R_base = matrix_from_xyz_quat((0.0, 0.0, 0.0), quat) + self.z_c = vertical_half_extent(self.R_base, self.object_dims) + self.table_size = doubles('table_size') + self.table_center = doubles('table_center') + self.return_center = doubles('return_shift_center') + self.return_bounds = doubles('return_shift_bounds_xy') + self.yaw_bound_deg = float(self.get_parameter('return_shift_bounds_yaw_deg').value) + self.initial_xy_yaw = doubles('initial_object_xy_yaw_deg') + self.min_place_distance = float(self.get_parameter('min_place_distance').value) + self.rng = np.random.default_rng(int(self.get_parameter('random_seed').value)) + self.group_name = self.get_parameter('group_name').value + self.end_effector_group = self.get_parameter('end_effector_group').value + self.tool_frame = self.get_parameter('tool_frame').value + self.planning_timeout_sec = float(self.get_parameter('planning_timeout_sec').value) + self.gripper_motion_duration_sec = float( + self.get_parameter('gripper_motion_duration_sec').value) + self.retract_dist = float(self.get_parameter('retract_dist_m').value) + self.surfaces = [int(value) for value in self.get_parameter('surfaces').value] + self.num_rotations = int(self.get_parameter('num_rotations').value) + self.ready_joints = { + name: float(self.get_parameter(f'ready_joints.{name}').value) for name in ARM_JOINTS + } + self.transit_scaling = float(self.get_parameter('transit_velocity_scaling').value) + self.cartesian_scaling = float(self.get_parameter('cartesian_velocity_scaling').value) + self.gripper_open = float(self.get_parameter('gripper_open').value) + self.gripper_closed = float(self.get_parameter('gripper_closed').value) + self.gripper_nominal_stroke = float(self.get_parameter('gripper_nominal_stroke').value) + self.gripper_max_opening = float(self.get_parameter('gripper_max_opening').value) + self.cycles_limit = int(self.get_parameter('cycles').value) + self.pause_sec = float(self.get_parameter('pause_between_phases_sec').value) + self.motion_plan_service = self.get_parameter('motion_plan_service').value + commit = self.get_parameter('intrinsic_moveit_commit').value or PINNED_COMMIT + base = self.get_parameter('robot_description_web_base').value + self.web_base = base.format(commit=commit) if '{commit}' in base else base + self.get_logger().info( + f'object half-height z_c={self.z_c:.4f} web URDF base {self.web_base}') + + def _on_joint_states(self, msg): + with self._lock: + for name, position in zip(msg.name, msg.position): + self._joints[name] = float(position) + self._have_joints.set() + + def _on_robot_description(self, msg): + rewritten = msg.data.replace('package://robot_hardware_description/', self.web_base) + out = String() + out.data = rewritten + self.web_urdf_pub.publish(out) + self.get_logger().info( + f'published /robot_description_web ({len(rewritten)} bytes, commit base {self.web_base})') + + def _joint_snapshot(self): + with self._lock: + return dict(self._joints) + + def _robot_state(self): + state = RobotState() + state.is_diff = True + joints = self._joint_snapshot() + state.joint_state.name = list(joints.keys()) + state.joint_state.position = [float(joints[name]) for name in state.joint_state.name] + return state + + def _publish_tf(self): + with self._lock: + if not self._tf_enabled: + return + if self._attached: + parent = self.tool_frame + transform = self._T_tcp_obj + else: + parent = 'world' + transform = self._T_world_obj + stamped = TransformStamped() + stamped.header.stamp = self.get_clock().now().to_msg() + stamped.header.frame_id = parent + stamped.child_frame_id = self.object_id + stamped.transform = matrix_to_transform(transform) + self.tf_broadcaster.sendTransform(stamped) + + def _publish_scene(self): + with self._lock: + ghost = self._ghost_pose + array = build_scene_markers( + self.get_clock().now().to_msg(), + 'world', + self.object_id, + self.object_dims, + self.object_label, + self.table_size, + self.table_center, + self.return_center, + self.return_bounds, + ghost, + ) + self.scene_pub.publish(array) + + def _publish_counters(self): + self.cycle_pub.publish(Int32(data=int(self.cycle))) + self.failures_pub.publish(Int32(data=int(self.failures))) + + def _set_status(self, text): + self.get_logger().info(text) + self.status_pub.publish(String(data=text)) + self._publish_counters() + + def _call(self, client, request, timeout): + done = threading.Event() + holder = {} + + def _finish(future): + try: + holder['result'] = future.result() + except Exception as exc: # noqa: BLE001 + holder['error'] = exc + done.set() + + future = client.call_async(request) + future.add_done_callback(_finish) + if not done.wait(timeout): + raise TimeoutError(f'{client.srv_name} timed out after {timeout:.1f}s') + if 'error' in holder: + raise holder['error'] + return holder['result'] + + def _send_goal(self, client, goal, timeout): + accepted = threading.Event() + holder = {} + + def _accepted(future): + try: + holder['handle'] = future.result() + except Exception as exc: # noqa: BLE001 + holder['error'] = exc + accepted.set() + + send_future = client.send_goal_async(goal) + send_future.add_done_callback(_accepted) + if not accepted.wait(min(timeout, 20.0)): + raise TimeoutError('timed out waiting for goal acceptance') + if 'error' in holder: + raise holder['error'] + handle = holder['handle'] + if not handle.accepted: + raise RuntimeError('goal rejected') + + finished = threading.Event() + result_holder = {} + + def _result(future): + try: + result_holder['result'] = future.result() + except Exception as exc: # noqa: BLE001 + result_holder['error'] = exc + finished.set() + + result_future = handle.get_result_async() + result_future.add_done_callback(_result) + if not finished.wait(timeout): + raise TimeoutError('timed out waiting for goal result') + if 'error' in result_holder: + raise result_holder['error'] + return result_holder['result'].result + + def _wait_interfaces(self, timeout=180.0): + services = [ + (self.apply_scene, '/apply_planning_scene'), + (self.get_scene, '/get_planning_scene'), + (self.plan_grasps, '/grasp_planning/plan_grasps'), + (self.motion_plan, self.motion_plan_service), + (self.cartesian, '/compute_cartesian_path'), + (self.ik, '/compute_ik'), + (self.fk, '/compute_fk'), + ] + actions = [ + (self.execute, '/execute_trajectory'), + (self.gripper, '/hand_controller/gripper_cmd'), + (self.follow_joints, '/ur_manipulator_controller/follow_joint_trajectory'), + ] + deadline = time.monotonic() + timeout + next_log = 0.0 + while rclpy.ok() and time.monotonic() < deadline: + missing = [name for client, name in services if not client.service_is_ready()] + missing += [name for client, name in actions if not client.server_is_ready()] + if not missing and self._have_joints.is_set(): + self.get_logger().info('planning interfaces and joint states are ready') + return + now = time.monotonic() + if now >= next_log: + joints = 'joint_states' if not self._have_joints.is_set() else None + waiting = missing + ([joints] if joints else []) + self.get_logger().info('waiting for: ' + ', '.join(waiting)) + next_log = now + 5.0 + time.sleep(0.2) + raise TimeoutError('timed out waiting for planning interfaces') + + def _object_pose(self, x, y, yaw): + return translation(x, y, self.z_c) @ rotation_z(yaw) @ self.R_base + + def _sample_pose(self, current_xy, enforce_distance): + cx, cy = self.return_center + bx, by = self.return_bounds + yaw_limit = math.radians(self.yaw_bound_deg) + for _ in range(40): + x = float(self.rng.uniform(cx - bx, cx + bx)) + y = float(self.rng.uniform(cy - by, cy + by)) + yaw = float(self.rng.uniform(-yaw_limit, yaw_limit)) + if not enforce_distance or math.hypot(x - current_xy[0], y - current_xy[1]) >= self.min_place_distance: + return self._object_pose(x, y, yaw) + raise RuntimeError('could not sample a place pose inside the return-shift window') + + def _collision_box(self, object_id, dims, transform, operation): + obj = CollisionObject() + obj.header.frame_id = 'world' + obj.header.stamp = self.get_clock().now().to_msg() + obj.id = object_id + obj.operation = operation + obj.pose.orientation.w = 1.0 + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = [float(value) for value in dims] + obj.primitives.append(primitive) + obj.primitive_poses.append(matrix_to_pose(transform)) + return obj + + def _apply(self, scene): + request = ApplyPlanningScene.Request() + request.scene = scene + response = self._call(self.apply_scene, request, 20.0) + if response is None or not response.success: + raise RuntimeError('apply_planning_scene failed') + return response + + def _add_world_box(self, object_id, dims, transform): + scene = PlanningScene() + scene.is_diff = True + scene.world.collision_objects.append( + self._collision_box(object_id, dims, transform, CollisionObject.ADD)) + self._apply(scene) + + def _remove_world_object(self, object_id): + scene = PlanningScene() + scene.is_diff = True + obj = CollisionObject() + obj.id = object_id + obj.operation = CollisionObject.REMOVE + scene.world.collision_objects.append(obj) + self._apply(scene) + + def _world_object_present(self): + request = GetPlanningScene.Request() + request.components.components = PlanningSceneComponents.WORLD_OBJECT_GEOMETRY + response = self._call(self.get_scene, request, 10.0) + if response is None: + return False + return any(obj.id == self.object_id for obj in response.scene.world.collision_objects) + + def _attach(self): + attached = AttachedCollisionObject() + attached.link_name = self.tool_frame + attached.touch_links = list(TOUCH_LINKS) + attached.object.id = self.object_id + attached.object.operation = CollisionObject.ADD + scene = PlanningScene() + scene.is_diff = True + scene.robot_state.is_diff = True + scene.robot_state.attached_collision_objects.append(attached) + self._apply(scene) + + def _detach(self): + attached = AttachedCollisionObject() + attached.link_name = self.tool_frame + attached.object.id = self.object_id + attached.object.operation = CollisionObject.REMOVE + scene = PlanningScene() + scene.is_diff = True + scene.robot_state.is_diff = True + scene.robot_state.attached_collision_objects.append(attached) + self._apply(scene) + + def _gripper(self, position): + goal = GripperCommand.Goal() + goal.command.position = float(position) + goal.command.max_effort = 50.0 + result = self._send_goal(self.gripper, goal, 15.0) + self.get_logger().info( + f'gripper command {position:.4f} reached={getattr(result, "reached_goal", None)} ' + f'position={getattr(result, "position", None)}') + return result + + def _joint_state_from_map(self, joint_map): + state = JointState() + state.name = list(ARM_JOINTS) + state.position = [float(joint_map[name]) for name in ARM_JOINTS] + return state + + def _plan_joint_goal(self, joint_state): + last_code = None + for use_start in (True, False): + request = GetMotionPlan.Request() + motion = request.motion_plan_request + motion.group_name = self.group_name + motion.pipeline_id = 'ompl' + motion.num_planning_attempts = 5 + motion.allowed_planning_time = 5.0 + motion.max_velocity_scaling_factor = self.transit_scaling + motion.max_acceleration_scaling_factor = self.transit_scaling + if use_start and self._joint_snapshot(): + motion.start_state = self._robot_state() + constraints = Constraints() + for name, position in zip(joint_state.name, joint_state.position): + constraints.joint_constraints.append(JointConstraint( + joint_name=name, + position=float(position), + tolerance_above=1e-3, + tolerance_below=1e-3, + weight=1.0, + )) + motion.goal_constraints.append(constraints) + response = self._call(self.motion_plan, request, 30.0) + code = response.motion_plan_response.error_code.val + points = response.motion_plan_response.trajectory.joint_trajectory.points + self.get_logger().info( + f'joint plan code={code} points={len(points)} explicit_start={use_start}') + if code == MoveItErrorCodes.SUCCESS and points: + return response.motion_plan_response.trajectory + last_code = code + raise RuntimeError(f'joint motion plan failed ({last_code})') + + def _publish_tcp_path(self, trajectory): + joint_trajectory = trajectory.joint_trajectory + points = list(joint_trajectory.points) + if not points: + return + if len(points) > 30: + indexes = [int(round(i * (len(points) - 1) / 29.0)) for i in range(30)] + else: + indexes = list(range(len(points))) + path = Path() + path.header.frame_id = 'world' + path.header.stamp = self.get_clock().now().to_msg() + for index in indexes: + point = points[index] + request = GetPositionFK.Request() + request.header.frame_id = 'world' + request.header.stamp = path.header.stamp + request.fk_link_names = [self.tool_frame] + request.robot_state.joint_state.name = list(joint_trajectory.joint_names) + request.robot_state.joint_state.position = [float(value) for value in point.positions] + response = self._call(self.fk, request, 10.0) + if response.error_code.val != MoveItErrorCodes.SUCCESS or not response.pose_stamped: + continue + path.poses.append(response.pose_stamped[0]) + if path.poses: + self.path_pub.publish(path) + + def _execute_trajectory(self, trajectory, timeout=60.0): + try: + self._publish_tcp_path(trajectory) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f'TCP path visualization failed: {exc}') + before = self._joint_snapshot() + goal = ExecuteTrajectory.Goal() + goal.trajectory = trajectory + result = self._send_goal(self.execute, goal, timeout) + code = result.error_code.val if result is not None else None + after = self._joint_snapshot() + deltas = { + name: round(after.get(name, 0.0) - before.get(name, 0.0), 3) for name in ARM_JOINTS + } + self.get_logger().info(f'execute code={code} arm_delta={deltas}') + if code != MoveItErrorCodes.SUCCESS: + raise RuntimeError(f'execute_trajectory failed ({code})') + return result + + def _move_joints(self, joint_map, allow_direct=True): + try: + trajectory = self._plan_joint_goal(self._joint_state_from_map(joint_map)) + self._execute_trajectory(trajectory) + return + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f'planned joint move failed: {exc}') + if not allow_direct: + raise + goal = FollowJointTrajectory.Goal() + goal.trajectory.joint_names = list(ARM_JOINTS) + point = JointTrajectoryPoint() + point.positions = [float(joint_map[name]) for name in ARM_JOINTS] + point.time_from_start = _duration(3.0) + goal.trajectory.points.append(point) + result = self._send_goal(self.follow_joints, goal, 20.0) + error_code = getattr(getattr(result, 'error_code', None), 'val', 0) + self.get_logger().info(f'direct joint trajectory error_code={error_code}') + if error_code not in (0, MoveItErrorCodes.SUCCESS): + raise RuntimeError(f'follow_joint_trajectory failed ({error_code})') + + def _cartesian_to(self, transform, avoid): + pose = matrix_to_pose(transform) + last = None + for use_start in (True, False): + request = GetCartesianPath.Request() + request.header.frame_id = 'world' + request.header.stamp = self.get_clock().now().to_msg() + request.group_name = self.group_name + request.link_name = self.tool_frame + request.waypoints.append(pose) + request.max_step = 0.005 + request.avoid_collisions = bool(avoid) + request.max_velocity_scaling_factor = self.cartesian_scaling + request.max_acceleration_scaling_factor = self.cartesian_scaling + if use_start and self._joint_snapshot(): + request.start_state = self._robot_state() + response = self._call(self.cartesian, request, 30.0) + points = len(response.solution.joint_trajectory.points) + self.get_logger().info( + f'cartesian avoid={avoid} fraction={response.fraction:.3f} ' + f'code={response.error_code.val} points={points} explicit_start={use_start}') + last = response + if response.fraction >= 0.95 and points > 0: + return response + return last + + def _execute_cartesian(self, transform, avoid, fallback_joints=None): + response = self._cartesian_to(transform, avoid) + if response.fraction < 0.95 or not response.solution.joint_trajectory.points: + if avoid: + self.get_logger().warn('cartesian fraction low, retrying with collisions ignored') + response = self._cartesian_to(transform, False) + if response.fraction >= 0.95 and response.solution.joint_trajectory.points: + self._execute_trajectory(response.solution) + return + if fallback_joints is None: + raise RuntimeError(f'cartesian path fraction {response.fraction:.3f}') + self.get_logger().warn('cartesian path incomplete, interpolating joint goal') + self._execute_trajectory(self._interpolate(fallback_joints)) + + def _interpolate(self, target_state, duration=2.0, samples=20): + current = self._joint_snapshot() + names = list(target_state.name) + target = [float(value) for value in target_state.position] + start = [float(current[name]) for name in names] + trajectory = JointTrajectory() + trajectory.joint_names = names + for index in range(samples): + alpha = float(index + 1) / float(samples) + point = JointTrajectoryPoint() + point.positions = [ + start[i] + alpha * (target[i] - start[i]) for i in range(len(names)) + ] + point.time_from_start = _duration(duration * alpha) + trajectory.points.append(point) + robot_trajectory = RobotTrajectory() + robot_trajectory.joint_trajectory = trajectory + return robot_trajectory + + def _fk_pose(self, joint_map): + request = GetPositionFK.Request() + request.header.frame_id = 'world' + request.fk_link_names = [self.tool_frame] + request.robot_state.joint_state.name = list(joint_map.keys()) + request.robot_state.joint_state.position = [float(joint_map[name]) for name in joint_map] + response = self._call(self.fk, request, 15.0) + if response.error_code.val != MoveItErrorCodes.SUCCESS or not response.pose_stamped: + raise RuntimeError(f'compute_fk failed ({response.error_code.val})') + return response.pose_stamped[0].pose + + def _validate_ready(self): + pose = self._fk_pose(self.ready_joints) + tool_z = quat_to_mat(( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))[:, 2] + self.get_logger().info( + f'ready hande_tcp xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f}) ' + f'tool_z={float(tool_z[2]):.3f}') + if float(tool_z[2]) > -0.3 or pose.position.z < 0.05: + self.get_logger().warn('ready pose failed the TCP check, using SRDF home') + self.ready_joints = dict(HOME_JOINTS) + pose = self._fk_pose(self.ready_joints) + self.get_logger().info( + f'home hande_tcp xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f})') + + def _publish_candidates(self, displays): + self.candidate_pub.publish(build_candidate_markers( + self.get_clock().now().to_msg(), self.object_id, displays)) + + def _select_candidates(self, response): + rotation_world = self._T_world_obj[:3, :3] + groups = {} + for index, grasp in enumerate(response.grasps): + pose = grasp.grasp_pose.pose + key = pose_key(pose) + rotation = quat_to_mat(( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) + width = sum(abs(float(rotation[axis, 0])) * self.object_dims[axis] for axis in range(3)) + approach_z = float((rotation_world @ rotation[:, 2])[2]) + item = { + 'index': index, + 'grasp': grasp, + 'pre_pose': response.pre_grasp_poses[index], + 'pre_ik': response.pregrasp_ik_solutions[index], + 'grasp_ik': response.grasp_ik_solutions[index], + 'width': width, + 'feasible': width <= self.gripper_max_opening + 1e-6, + 'approach_z': approach_z, + 'quality': float(grasp.grasp_quality), + 'key': key, + 'pose': pose, + } + groups.setdefault(key, []).append(item) + + def best_feasible(items): + feasible = [item for item in items if item['feasible']] + if not feasible: + return None + return max(feasible, key=lambda item: (item['quality'], -item['approach_z'])) + + ordered_keys = sorted(groups, key=lambda key: ( + best_feasible(groups[key]) is None, + -(best_feasible(groups[key])['quality'] if best_feasible(groups[key]) else max( + item['quality'] for item in groups[key])), + min(item['approach_z'] for item in groups[key]), + )) + displays = [] + feasible_rank = 0 + for index, key in enumerate(ordered_keys): + items = groups[key] + chosen = best_feasible(items) or max(items, key=lambda item: item['quality']) + pre = chosen['pre_pose'].pose.position + grasp_position = chosen['pose'].position + displays.append({ + 'pose': chosen['pose'], + 'pre_position': (pre.x, pre.y, pre.z), + 'grasp_position': (grasp_position.x, grasp_position.y, grasp_position.z), + 'width': chosen['width'], + 'quality': chosen['quality'], + 'n_ik': len(items), + 'feasible': chosen['feasible'], + 'selected': False, + 'rank': feasible_rank if chosen['feasible'] else 0, + }) + if chosen['feasible']: + feasible_rank += 1 + + tries = [] + for key in ordered_keys: + chosen = best_feasible(groups[key]) + if chosen is not None: + tries.append(chosen) + tries = tries[:3] + if displays and tries: + selected_key = tries[0]['key'] + for display, key in zip(displays, ordered_keys): + display['selected'] = key == selected_key + return displays, tries + + def _publish_selection(self, candidate): + grasp = candidate['grasp'] + stamped = PoseStamped() + stamped.header.frame_id = self.object_id + stamped.header.stamp = self.get_clock().now().to_msg() + stamped.pose = grasp.grasp_pose.pose + self.selected_pose_pub.publish(stamped) + self.selected_msg_pub.publish(grasp) + + def _phase_init(self): + self._wait_interfaces() + self._add_world_box('table', self.table_size, translation(*self.table_center)) + self._validate_ready() + self._move_joints(self.ready_joints, allow_direct=True) + self._gripper(self.gripper_closed) + return 'SPAWN_WORKPIECE' + + def _phase_spawn(self): + if self._pending_pose is None: + x, y, yaw_deg = self.initial_xy_yaw + pose = self._object_pose(float(x), float(y), math.radians(float(yaw_deg))) + else: + pose = self._pending_pose + self._pending_pose = None + with self._lock: + self._T_world_obj = pose + self._attached = False + self._tf_enabled = True + self._ghost_pose = None + self._add_world_box(self.object_id, self.object_dims, pose) + time.sleep(0.2) + self._publish_scene() + self.get_logger().info( + f'spawned {self.object_id} at ({pose[0, 3]:.3f}, {pose[1, 3]:.3f}, {pose[2, 3]:.3f})') + return 'PLAN_GRASPS' + + def _phase_plan(self): + request = PlanGrasps.Request() + request.group_name = self.group_name + request.end_effector_group = self.end_effector_group + request.tool_frame = self.tool_frame + request.planning_timeout_sec = self.planning_timeout_sec + request.gripper_motion_duration_sec = self.gripper_motion_duration_sec + request.retract_dist_m = self.retract_dist + request.surfaces = list(self.surfaces) + request.num_rotations = self.num_rotations + request.target.id = self.object_id + started = time.monotonic() + response = self._call(self.plan_grasps, request, self.planning_timeout_sec + 30.0) + latency = time.monotonic() - started + count = 0 if response is None else len(response.grasps) + code = None if response is None else response.error_code.val + self.latency_pub.publish(Float64(data=float(latency))) + self.count_pub.publish(Int32(data=int(count))) + self.get_logger().info(f'returned {count} candidates in {latency:.3f}s code={code}') + if response is None or code != MoveItErrorCodes.SUCCESS or count == 0: + raise RuntimeError(f'plan_grasps failed code={code} count={count}') + displays, tries = self._select_candidates(response) + if not tries: + raise RuntimeError('no feasible grasp candidates') + self._displays = displays + self._tries = tries + self._selected = tries[0] + poses = PoseArray() + poses.header.frame_id = self.object_id + poses.header.stamp = self.get_clock().now().to_msg() + pre_poses = PoseArray() + pre_poses.header = poses.header + for grasp, pre in zip(response.grasps, response.pre_grasp_poses): + poses.poses.append(grasp.grasp_pose.pose) + pre_poses.poses.append(pre.pose) + self.grasp_poses_pub.publish(poses) + self.pregrasp_poses_pub.publish(pre_poses) + self._publish_candidates(displays) + self.get_logger().info( + f'{len(displays)} distinct poses, {len(tries)} feasible fallbacks, ' + f'best q={tries[0]["quality"]:.3f} width={tries[0]["width"]*1000:.1f}mm ' + f'approach_z={tries[0]["approach_z"]:+.2f}') + return 'SELECT_GRASP' + + def _phase_select(self): + self._publish_selection(self._selected) + self.get_logger().info( + f'selected grasp id={self._selected["grasp"].id} q={self._selected["quality"]:.3f}') + return 'OPEN_GRIPPER' + + def _open_position(self, grasp): + points = grasp.pre_grasp_posture.points + if points and points[-1].positions: + return float(points[-1].positions[0]) + return self.gripper_open + + def _phase_open(self): + self._gripper(self._open_position(self._selected['grasp'])) + return 'MOVE_TO_PREGRASP' + + def _phase_pregrasp(self): + last_error = None + for index, candidate in enumerate(self._tries): + self._selected = candidate + for display in self._displays: + display['selected'] = False + if self._displays: + self._displays[0]['selected'] = index == 0 + for display in self._displays: + if abs(display['quality'] - candidate['quality']) < 1e-9 and abs( + display['width'] - candidate['width']) < 1e-9: + display['selected'] = True + break + self._publish_candidates(self._displays) + self._publish_selection(candidate) + try: + self._execute_trajectory(self._plan_joint_goal(candidate['pre_ik']), timeout=60.0) + return 'APPROACH' + except Exception as exc: # noqa: BLE001 + last_error = exc + self.get_logger().warn(f'pregrasp candidate {index} failed: {exc}') + raise RuntimeError(f'all pregrasp attempts failed: {last_error}') + + def _phase_approach(self): + grasp_pose = pose_to_matrix(self._selected['pose']) + world_grasp = self._T_world_obj @ grasp_pose + self._execute_cartesian(world_grasp, avoid=False, fallback_joints=self._selected['grasp_ik']) + return 'GRASP' + + def _phase_grasp(self): + width = self._selected['width'] + close = (self.gripper_nominal_stroke - width) / 2.0 + close = min(max(close, self.gripper_open), self.gripper_closed) + self._gripper(close) + self._attach() + tcp_object = invert(pose_to_matrix(self._selected['pose'])) + with self._lock: + self._T_tcp_obj = tcp_object + self._attached = True + kept = [item for item in self._displays if item['selected']] or self._displays[:1] + for item in kept: + item['selected'] = True + self._publish_candidates(kept) + self.get_logger().info(f'attached {self.object_id}, finger command {close:.4f}') + return 'RETREAT' + + def _phase_retreat(self): + grasp_pose = pose_to_matrix(self._selected['pose']) + world_grasp = self._T_world_obj @ grasp_pose + retreat = world_grasp @ translation(0.0, 0.0, -self.retract_dist) + self._execute_cartesian(retreat, avoid=True) + return 'PLACE' + + def _phase_place(self): + current_xy = (float(self._T_world_obj[0, 3]), float(self._T_world_obj[1, 3])) + grasp_in_object = pose_to_matrix(self._selected['pose']) + last_error = None + for attempt in range(5): + target = self._sample_pose(current_xy, enforce_distance=True) + place_tcp = target @ grasp_in_object + pre_place = place_tcp @ translation(0.0, 0.0, -self.retract_dist) + try: + joints = self._ik_pose(pre_place) + with self._lock: + self._ghost_pose = matrix_to_pose(target) + self._place_target = target + self._publish_scene() + self._execute_trajectory(self._plan_joint_goal(joints)) + return 'PLACE_DESCEND' + except Exception as exc: # noqa: BLE001 + last_error = exc + self.get_logger().warn(f'place sample {attempt} failed: {exc}') + raise RuntimeError(f'place planning failed: {last_error}') + + def _ik_pose(self, transform): + request = GetPositionIK.Request() + request.ik_request.group_name = self.group_name + request.ik_request.ik_link_name = self.tool_frame + request.ik_request.pose_stamped.header.frame_id = 'world' + request.ik_request.pose_stamped.header.stamp = self.get_clock().now().to_msg() + request.ik_request.pose_stamped.pose = matrix_to_pose(transform) + request.ik_request.robot_state = self._robot_state() + request.ik_request.avoid_collisions = True + request.ik_request.timeout = _duration(0.5) + response = self._call(self.ik, request, 10.0) + if response.error_code.val != MoveItErrorCodes.SUCCESS: + raise RuntimeError(f'compute_ik failed ({response.error_code.val})') + positions = dict(zip(response.solution.joint_state.name, response.solution.joint_state.position)) + missing = [name for name in ARM_JOINTS if name not in positions] + if missing: + raise RuntimeError(f'IK solution missing {missing}') + return self._joint_state_from_map({name: positions[name] for name in ARM_JOINTS}) + + def _phase_descend(self): + grasp_in_object = pose_to_matrix(self._selected['pose']) + place_tcp = self._place_target @ grasp_in_object + self._execute_cartesian(place_tcp, avoid=False) + return 'RELEASE' + + def _phase_release(self): + self._gripper(self.gripper_open) + self._detach() + self._add_world_box(self.object_id, self.object_dims, self._place_target) + if not self._world_object_present(): + self._add_world_box(self.object_id, self.object_dims, self._place_target) + with self._lock: + self._T_world_obj = self._place_target + self._attached = False + self._ghost_pose = None + self._publish_scene() + self.get_logger().info('released workpiece at the sampled place pose') + return 'RETREAT_UP' + + def _phase_retreat_up(self): + grasp_in_object = pose_to_matrix(self._selected['pose']) + place_tcp = self._T_world_obj @ grasp_in_object + retreat = place_tcp @ translation(0.0, 0.0, -self.retract_dist) + self._execute_cartesian(retreat, avoid=True) + return 'PARK' + + def _phase_park(self): + self._gripper(self.gripper_closed) + self._move_joints(self.ready_joints, allow_direct=True) + self.cycle += 1 + self._publish_counters() + self.get_logger().info(f'cycle {self.cycle} complete') + if self.cycles_limit > 0 and self.cycle >= self.cycles_limit: + return 'DONE' + return 'PLAN_GRASPS' + + def _recover(self, reason): + self.failures += 1 + self._set_status(f'RECOVER: {reason}') + self.get_logger().error(f'RECOVER: {reason}') + try: + if self._attached: + self._detach() + self._remove_world_object(self.object_id) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f'recover scene cleanup failed: {exc}') + with self._lock: + self._attached = False + self._ghost_pose = None + self._tf_enabled = False + self._publish_candidates([]) + self._publish_scene() + try: + self._gripper(self.gripper_open) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f'recover gripper open failed: {exc}') + try: + self._move_joints(self.ready_joints, allow_direct=True) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f'recover return to ready failed: {exc}') + try: + current = self._joint_snapshot() + current_xy = (float(self._T_world_obj[0, 3]), float(self._T_world_obj[1, 3])) + self._pending_pose = self._sample_pose(current_xy, enforce_distance=False) + del current + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f'recover resample failed: {exc}') + x, y, yaw_deg = self.initial_xy_yaw + self._pending_pose = self._object_pose( + float(x), float(y), math.radians(float(yaw_deg))) + return 'SPAWN_WORKPIECE' + + def run(self): + handlers = { + 'INIT': self._phase_init, + 'SPAWN_WORKPIECE': self._phase_spawn, + 'PLAN_GRASPS': self._phase_plan, + 'SELECT_GRASP': self._phase_select, + 'OPEN_GRIPPER': self._phase_open, + 'MOVE_TO_PREGRASP': self._phase_pregrasp, + 'APPROACH': self._phase_approach, + 'GRASP': self._phase_grasp, + 'RETREAT': self._phase_retreat, + 'PLACE': self._phase_place, + 'PLACE_DESCEND': self._phase_descend, + 'RELEASE': self._phase_release, + 'RETREAT_UP': self._phase_retreat_up, + 'PARK': self._phase_park, + } + phase = 'INIT' + while rclpy.ok(): + if phase == 'DONE': + self._set_status('DONE') + while rclpy.ok(): + time.sleep(0.5) + return + self._set_status(phase) + started = time.monotonic() + try: + phase = handlers[phase]() + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f'{phase} failed: {exc}\n{traceback.format_exc()}') + try: + phase = self._recover(f'{phase}: {exc}') + except Exception as recover_exc: # noqa: BLE001 + self.get_logger().error(f'recover failed: {recover_exc}') + self.failures += 1 + self._publish_counters() + time.sleep(1.0) + phase = 'SPAWN_WORKPIECE' + else: + self.get_logger().info(f'{phase} next after {time.monotonic() - started:.1f}s') + if phase != 'DONE': + time.sleep(self.pause_sec) + + +def main(): + rclpy.init() + node = GraspDemoDriver() + executor = MultiThreadedExecutor() + executor.add_node(node) + spinner = threading.Thread(target=executor.spin, daemon=True) + spinner.start() + try: + node.run() + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/markers.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/markers.py new file mode 100644 index 0000000..582616f --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/markers.py @@ -0,0 +1,198 @@ +from geometry_msgs.msg import Point +from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker, MarkerArray + +from intrinsic_foxglove_demo.geometry import matrix_to_pose, pose_to_matrix, translation + + +def _color(hex_rgb, alpha=1.0): + text = hex_rgb.lstrip('#') + return ColorRGBA( + r=int(text[0:2], 16) / 255.0, + g=int(text[2:4], 16) / 255.0, + b=int(text[4:6], 16) / 255.0, + a=float(alpha), + ) + + +def _lerp(a, b, t): + return tuple(a[i] + (b[i] - a[i]) * t for i in range(3)) + + +def _rank_color(rank, count): + if count <= 1: + mix = 0.0 + else: + mix = rank / float(count - 1) + green = (0.0, 0.90, 0.29) + yellow = (1.0, 0.86, 0.0) + orange = (1.0, 0.55, 0.0) + if mix < 0.5: + rgb = _lerp(green, yellow, mix / 0.5) + else: + rgb = _lerp(yellow, orange, (mix - 0.5) / 0.5) + return ColorRGBA(r=rgb[0], g=rgb[1], b=rgb[2], a=0.95) + + +def _base(frame, namespace, marker_id, stamp): + marker = Marker() + marker.header.frame_id = frame + marker.header.stamp = stamp + marker.ns = namespace + marker.id = marker_id + marker.action = Marker.ADD + marker.pose.orientation.w = 1.0 + return marker + + +def build_scene_markers( + stamp, + world_frame, + object_frame, + object_dims, + object_label, + table_size, + table_center, + return_center_xy, + return_bounds_xy, + ghost_pose, +): + array = MarkerArray() + table = _base(world_frame, 'scene', 1, stamp) + table.type = Marker.CUBE + table.pose.position.x = float(table_center[0]) + table.pose.position.y = float(table_center[1]) + table.pose.position.z = float(table_center[2]) + table.scale.x = float(table_size[0]) + table.scale.y = float(table_size[1]) + table.scale.z = float(table_size[2]) + table.color = _color('4a4f57') + array.markers.append(table) + + table_top = float(table_center[2]) + 0.5 * float(table_size[2]) + cx, cy = float(return_center_xy[0]), float(return_center_xy[1]) + bx, by = float(return_bounds_xy[0]), float(return_bounds_xy[1]) + outline = _base(world_frame, 'scene', 2, stamp) + outline.type = Marker.LINE_STRIP + outline.scale.x = 0.006 + outline.color = _color('e8eef5') + z_line = table_top + 0.002 + corners = [ + (cx - bx, cy - by), + (cx + bx, cy - by), + (cx + bx, cy + by), + (cx - bx, cy + by), + (cx - bx, cy - by), + ] + for x, y in corners: + outline.points.append(Point(x=x, y=y, z=z_line)) + array.markers.append(outline) + + pad = _base(world_frame, 'scene', 3, stamp) + pad.type = Marker.CUBE + pad.pose.position.x = cx + pad.pose.position.y = cy + pad.pose.position.z = table_top + 0.001 + pad.scale.x = 2.0 * bx + pad.scale.y = 2.0 * by + pad.scale.z = 0.002 + pad.color = _color('c5d0dc', 0.35) + array.markers.append(pad) + + workpiece = _base(object_frame, 'scene', 4, stamp) + workpiece.type = Marker.CUBE + workpiece.scale.x = float(object_dims[0]) + workpiece.scale.y = float(object_dims[1]) + workpiece.scale.z = float(object_dims[2]) + workpiece.color = _color('b8bcc2') + workpiece.frame_locked = True + array.markers.append(workpiece) + + label = _base(object_frame, 'scene', 5, stamp) + label.type = Marker.TEXT_VIEW_FACING + label.pose.position.z = 0.1 + label.scale.z = 0.02 + label.color = _color('f5f7fa') + label.text = object_label + label.frame_locked = True + array.markers.append(label) + + ghost = _base(world_frame, 'scene', 6, stamp) + if ghost_pose is None: + ghost.action = Marker.DELETE + else: + ghost.type = Marker.CUBE + ghost.pose = ghost_pose + ghost.scale.x = float(object_dims[0]) + ghost.scale.y = float(object_dims[1]) + ghost.scale.z = float(object_dims[2]) + ghost.color = _color('00e5ff', 0.25) + array.markers.append(ghost) + return array + + +def build_candidate_markers(stamp, object_frame, displays): + array = MarkerArray() + clear = _base(object_frame, 'grasp', 0, stamp) + clear.action = Marker.DELETEALL + array.markers.append(clear) + + feasible_count = sum(1 for item in displays if item['feasible']) + for index, item in enumerate(displays): + pose = item['pose'] + transform = pose_to_matrix(pose) + scale = 1.3 if item['selected'] else 1.0 + if item['selected']: + color = _color('00e676') + elif item['feasible']: + color = _rank_color(item['rank'], max(feasible_count, 1)) + else: + color = _color('ff1744', 0.3) + base_id = 10 + index * 10 + + palm = _base(object_frame, 'grasp', base_id + 1, stamp) + palm.type = Marker.CUBE + palm.pose = matrix_to_pose(transform @ translation(0.0, 0.0, -0.035)) + palm.scale.x = 0.07 * scale + palm.scale.y = 0.012 * scale + palm.scale.z = 0.012 * scale + palm.color = color + palm.frame_locked = True + array.markers.append(palm) + + half = 0.5 * float(item['width']) + 0.005 + for finger_id, sign in ((2, 1.0), (3, -1.0)): + finger = _base(object_frame, 'grasp', base_id + finger_id, stamp) + finger.type = Marker.CUBE + finger.pose = matrix_to_pose(transform @ translation(sign * half, 0.0, -0.0175)) + finger.scale.x = 0.01 * scale + finger.scale.y = 0.022 * scale + finger.scale.z = 0.035 * scale + finger.color = color + finger.frame_locked = True + array.markers.append(finger) + + arrow = _base(object_frame, 'grasp', base_id + 4, stamp) + arrow.type = Marker.ARROW + pre = item['pre_position'] + grasp = item['grasp_position'] + arrow.points.append(Point(x=float(pre[0]), y=float(pre[1]), z=float(pre[2]))) + arrow.points.append(Point(x=float(grasp[0]), y=float(grasp[1]), z=float(grasp[2]))) + arrow.scale.x = 0.004 * scale + arrow.scale.y = 0.008 * scale + arrow.scale.z = 0.012 * scale + arrow.color = color + arrow.frame_locked = True + array.markers.append(arrow) + + text = _base(object_frame, 'grasp', base_id + 5, stamp) + text.type = Marker.TEXT_VIEW_FACING + text.pose.position.x = float(grasp[0]) + text.pose.position.y = float(grasp[1]) + text.pose.position.z = float(grasp[2]) + 0.05 + text.scale.z = 0.012 * scale + text.color = color + text.text = f"grasp_{index} q={item['quality']:.3f} ({item['n_ik']} IK)" + text.frame_locked = True + array.markers.append(text) + return array diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/launch/demo.launch.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/launch/demo.launch.py new file mode 100644 index 0000000..3019b4f --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/launch/demo.launch.py @@ -0,0 +1,106 @@ +import os +from datetime import datetime + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +TOPICS = [ + '/robot_description', + '/robot_description_web', + '/robot_description_semantic', + '/tf', + '/tf_static', + '/joint_states', + '/ur_manipulator_controller/controller_state', + '/rosout', + '/demo/scene_markers', + '/demo/grasp_candidates', + '/demo/grasp_poses', + '/demo/pregrasp_poses', + '/demo/selected_grasp', + '/demo/selected_grasp_msg', + '/demo/planned_tcp_path', + '/demo/status', + '/demo/cycle', + '/demo/failures', + '/demo/grasp_planning/latency_s', + '/demo/grasp_planning/num_candidates', +] + + +def generate_launch_description(): + stamp = datetime.now().strftime('%Y%m%d_%H%M%S') + demo_share = get_package_share_directory('intrinsic_foxglove_demo') + service_share = get_package_share_directory('moveit_planning_service') + record = LaunchConfiguration('record') + recording_dir = LaunchConfiguration('recording_dir') + cycles = LaunchConfiguration('cycles') + bridge_port = LaunchConfiguration('bridge_port') + + service = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(service_share, 'launch', 'service.launch.py')), + launch_arguments={ + 'use_mock_hardware': 'true', + 'expect_collision_objects': 'false', + 'start_service_status_monitor': 'false', + 'headless': 'true', + }.items(), + ) + + bridge = Node( + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output='screen', + parameters=[{ + 'port': ParameterValue(bridge_port, value_type=int), + 'address': '0.0.0.0', + 'use_compression': False, + }], + ) + + driver = Node( + package='intrinsic_foxglove_demo', + executable='grasp_demo_driver', + name='grasp_demo_driver', + output='screen', + parameters=[ + os.path.join(demo_share, 'config', 'demo_params.yaml'), + { + 'cycles': ParameterValue(cycles, value_type=int), + 'intrinsic_moveit_commit': EnvironmentVariable( + 'INTRINSIC_MOVEIT_COMMIT', + default_value='c5e3290aa0f0e64c2d106a2fb4eb10cb52592205', + ), + }, + ], + ) + + bag = ExecuteProcess( + condition=IfCondition(record), + cmd=[ + 'ros2', 'bag', 'record', + '-s', 'mcap', + '--storage-preset-profile', 'zstd_fast', + '-o', PathJoinSubstitution([recording_dir, f'intrinsic_grasp_demo_{stamp}']), + '--topics', *TOPICS, + ], + output='screen', + ) + + return LaunchDescription([ + DeclareLaunchArgument('record', default_value='true'), + DeclareLaunchArgument('recording_dir', default_value='/recordings'), + DeclareLaunchArgument('cycles', default_value='0'), + DeclareLaunchArgument('bridge_port', default_value='8765'), + service, + bridge, + driver, + bag, + ]) diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/package.xml b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/package.xml new file mode 100644 index 0000000..36bf57c --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/package.xml @@ -0,0 +1,31 @@ + + + + intrinsic_foxglove_demo + 0.1.0 + Foxglove demo driver for Intrinsic MoveIt grasp planning on a mock UR5e and Robotiq Hand-E. + Foxglove + Apache-2.0 + + rclpy + moveit_msgs + moveit_planning_interfaces + control_msgs + visualization_msgs + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + shape_msgs + trajectory_msgs + tf2_ros + python3-numpy + launch_ros + foxglove_bridge + moveit_planning_service + rosbag2_storage_mcap + + + ament_python + + diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/resource/intrinsic_foxglove_demo b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/resource/intrinsic_foxglove_demo new file mode 100644 index 0000000..e69de29 diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.cfg b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.cfg new file mode 100644 index 0000000..887c7eb --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/intrinsic_foxglove_demo +[install] +install_scripts=$base/lib/intrinsic_foxglove_demo diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.py new file mode 100644 index 0000000..f3c4cf9 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'intrinsic_foxglove_demo' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Foxglove', + maintainer_email='support@foxglove.dev', + description='Foxglove demo driver for Intrinsic MoveIt grasp planning on mock hardware.', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'grasp_demo_driver = intrinsic_foxglove_demo.grasp_demo_driver:main', + ], + }, +) diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/CMakeLists.txt b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/CMakeLists.txt new file mode 100644 index 0000000..870136a --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(moveit_planning_service) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +set(INTRINSIC_MOVEIT_DIR "/opt/intrinsic-moveit" CACHE PATH "Checkout of intrinsic-ai/intrinsic-moveit") +set(UPSTREAM_DIR "${INTRINSIC_MOVEIT_DIR}/moveit_planning_service") +if(NOT EXISTS "${UPSTREAM_DIR}/src/grasp_planning_pipeline.cpp") + message(FATAL_ERROR "INTRINSIC_MOVEIT_DIR=${INTRINSIC_MOVEIT_DIR} does not contain moveit_planning_service sources") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(moveit_planning_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(shape_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +add_executable(moveit_planning_node + src/moveit_planning_node_standalone.cpp + ${UPSTREAM_DIR}/src/grasp_planning_pipeline.cpp + ${UPSTREAM_DIR}/src/generate_box_grasp_poses.cpp + ${UPSTREAM_DIR}/src/object_geometry.cpp +) +target_include_directories(moveit_planning_node PRIVATE ${UPSTREAM_DIR}/include) +target_link_libraries(moveit_planning_node + rclcpp::rclcpp + Eigen3::Eigen + ${moveit_msgs_TARGETS} + ${moveit_core_TARGETS} + ${moveit_ros_planning_TARGETS} + ${moveit_task_constructor_core_TARGETS} + ${moveit_planning_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${shape_msgs_TARGETS} + ${trajectory_msgs_TARGETS} +) + +install(TARGETS moveit_planning_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY ${UPSTREAM_DIR}/launch ${UPSTREAM_DIR}/rviz DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/package.xml b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/package.xml new file mode 100644 index 0000000..4143c15 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/package.xml @@ -0,0 +1,33 @@ + + + + moveit_planning_service + 0.0.1 + SDK-free standalone build of Intrinsic's moveit_planning_service (mock hardware mode only) + Foxglove + Apache-2.0 + + ament_cmake + + rclcpp + moveit_msgs + moveit_core + moveit_ros_planning + moveit_task_constructor_core + moveit_planning_interfaces + geometry_msgs + sensor_msgs + shape_msgs + trajectory_msgs + eigen + + robot_hardware_moveit_config + moveit_configs_utils + moveit_ros_move_group + robot_state_publisher + controller_manager + + + ament_cmake + + diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp new file mode 100644 index 0000000..9aa6adb --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp @@ -0,0 +1,133 @@ +// Copyright 2026 Intrinsic Innovation LLC +// +// 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 +// +// https://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. + +// Derived from intrinsic-ai/intrinsic-moveit +// moveit_planning_service/src/moveit_planning_node.cpp @ c5e3290; Flowstate World sync, +// Zenoh/pubsub, RuntimeContext proto and status monitor removed. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "moveit_planning_service/grasp_planning_pipeline.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" + +using PlanGrasps = moveit_planning_interfaces::srv::PlanGrasps; +using GetMotionPlan = moveit_msgs::srv::GetMotionPlan; +using GetPlanningScene = moveit_msgs::srv::GetPlanningScene; + +namespace { + +void handle_motion_plan_request( + const std::shared_ptr>& scene_ready, + const std::shared_ptr& request, + std::shared_ptr& response) { + if (!scene_ready->load()) { + RCLCPP_WARN(rclcpp::get_logger("moveit_planning_service"), + "Rejecting motion planning request: collision scene is not ready yet."); + response->motion_plan_response.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + RCLCPP_INFO(rclcpp::get_logger("moveit_planning_service"), + "Received motion planning request for group: '%s'", + request->motion_plan_request.group_name.c_str()); + // Upstream is a TODO stub as well: it returns SUCCESS with an empty trajectory. + response->motion_plan_response.group_name = request->motion_plan_request.group_name; + response->motion_plan_response.planning_time = 0.05; + response->motion_plan_response.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + RCLCPP_INFO(rclcpp::get_logger("moveit_planning_service"), + "Successfully generated dummy motion plan."); +} + +} // namespace + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("moveit_planning_node"); + + rcl_interfaces::msg::ParameterDescriptor double_desc; + double_desc.dynamic_typing = true; + node->declare_parameter("ros_service_call_timeout_sec", rclcpp::ParameterValue(5.0), + double_desc); + node->declare_parameter("expect_collision_objects", true); + node->declare_parameter("use_mock_hardware", false); + + if (!node->get_parameter("use_mock_hardware").as_bool()) { + RCLCPP_WARN(node->get_logger(), + "This standalone build only supports use_mock_hardware:=true " + "(no Intrinsic platform connection). Continuing in mock mode."); + } + auto scene_ready = std::make_shared>(true); + + auto planning_scene_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto planning_scene_client = node->create_client( + "/get_planning_scene", rclcpp::ServicesQoS(), planning_scene_callback_group); + auto planning_scene_executor = std::make_shared(); + planning_scene_executor->add_callback_group(planning_scene_callback_group, + node->get_node_base_interface()); + std::thread planning_scene_executor_thread( + [planning_scene_executor]() { planning_scene_executor->spin(); }); + + auto planning_services_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto motion_service = node->create_service( + "motion_planning/get_motion_plan", + [scene_ready](const std::shared_ptr request, + std::shared_ptr response) { + handle_motion_plan_request(scene_ready, request, response); + }, + rclcpp::ServicesQoS(), planning_services_callback_group); + + auto grasp_pipeline = std::make_shared( + node, planning_scene_client); + + auto grasp_service = node->create_service( + "grasp_planning/plan_grasps", + [node, grasp_pipeline](const PlanGrasps::Request::SharedPtr request, + PlanGrasps::Response::SharedPtr response) { + double timeout_sec = 5.0; + node->get_parameter("ros_service_call_timeout_sec", timeout_sec); + grasp_pipeline->PlanGrasps( + request, response, + std::chrono::milliseconds(static_cast(timeout_sec * 1000.0))); + }, + rclcpp::ServicesQoS(), planning_services_callback_group); + + RCLCPP_INFO(node->get_logger(), "MoveIt Planning Service (standalone) started."); + RCLCPP_INFO(node->get_logger(), " - motion_planning/get_motion_plan"); + RCLCPP_INFO(node->get_logger(), " - grasp_planning/plan_grasps"); + + rclcpp::experimental::executors::EventsExecutor executor; + executor.add_node(node); + executor.spin(); + + planning_scene_executor->cancel(); + if (planning_scene_executor_thread.joinable()) { + planning_scene_executor_thread.join(); + } + rclcpp::shutdown(); + return 0; +} diff --git a/integrations/ros2/intrinsic_moveit/scripts/check_foxglove_ws.py b/integrations/ros2/intrinsic_moveit/scripts/check_foxglove_ws.py new file mode 100755 index 0000000..17ec9eb --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/scripts/check_foxglove_ws.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import argparse +import asyncio +import json +import struct +import sys + +import websockets + +REQUIRED_TOPICS = { + '/robot_description', + '/robot_description_web', + '/tf', + '/tf_static', + '/joint_states', + '/demo/scene_markers', + '/demo/grasp_candidates', + '/demo/grasp_poses', + '/demo/pregrasp_poses', + '/demo/selected_grasp', + '/demo/planned_tcp_path', + '/demo/status', +} +ASSET_URI = 'package://robot_hardware_description/meshes/robotiq_hande/robotiq_hande_body.dae' + + +async def main(): + parser = argparse.ArgumentParser() + parser.add_argument('url', nargs='?', default='ws://localhost:8765') + args = parser.parse_args() + async with websockets.connect( + args.url, subprotocols=['foxglove.sdk.v1'], max_size=None) as ws: + server_info = json.loads(await ws.recv()) + capabilities = server_info.get('capabilities') or [] + print('serverInfo', server_info.get('op'), 'capabilities', capabilities) + if 'assets' not in capabilities: + raise SystemExit('FAIL capabilities missing assets') + + topics = {} + deadline = asyncio.get_event_loop().time() + 20.0 + while asyncio.get_event_loop().time() < deadline and not REQUIRED_TOPICS.issubset(topics): + try: + msg = await asyncio.wait_for(ws.recv(), timeout=2.0) + except asyncio.TimeoutError: + continue + if isinstance(msg, str): + payload = json.loads(msg) + if payload.get('op') == 'advertise': + for channel in payload.get('channels', []): + topics[channel['topic']] = channel['id'] + missing = sorted(REQUIRED_TOPICS - set(topics)) + print(f'advertised {len(topics)} topics') + if missing: + raise SystemExit(f'FAIL missing topics: {missing}') + + await ws.send(json.dumps({'op': 'fetchAsset', 'uri': ASSET_URI, 'requestId': 7})) + asset_status = None + asset_bytes = 0 + while True: + msg = await asyncio.wait_for(ws.recv(), timeout=30.0) + if isinstance(msg, (bytes, bytearray)) and msg and msg[0] == 0x04: + _req_id, asset_status = struct.unpack_from(' 1 else 30.0 + rclpy.init() + node = rclpy.create_node('check_motion') + samples = {name: [] for name in ARM_JOINTS + ['hande_left_finger_joint']} + statuses = [] + + def on_joints(msg): + for name, position in zip(msg.name, msg.position): + if name in samples: + samples[name].append(float(position)) + + def on_status(msg): + if not statuses or statuses[-1] != msg.data: + statuses.append(msg.data) + + latched = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + node.create_subscription(JointState, '/joint_states', on_joints, 50) + node.create_subscription(String, '/demo/status', on_status, latched) + deadline = time.monotonic() + duration + while rclpy.ok() and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + spans = {} + for name, values in samples.items(): + if values: + spans[name] = max(values) - min(values) + else: + spans[name] = 0.0 + moved = [name for name in ARM_JOINTS if spans[name] > 0.5] + print('joint spans', {name: round(spans[name], 4) for name in spans}) + print('statuses', statuses) + if len(moved) < 3: + raise SystemExit(f'FAIL only {len(moved)} arm joints moved more than 0.5 rad') + if spans['hande_left_finger_joint'] <= 0.005: + raise SystemExit('FAIL gripper did not move') + missing = [phase for phase in REQUIRED_PHASES if not any(phase in text for text in statuses)] + if missing: + raise SystemExit(f'FAIL status missing {missing}') + print( + f'PASS motion arm_joints={len(moved)} ' + f'gripper_span={spans["hande_left_finger_joint"]:.4f} phases={len(REQUIRED_PHASES)}') + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + try: + main() + except SystemExit: + raise + except Exception as exc: + print(f'FAIL {exc}', file=sys.stderr) + raise SystemExit(1) from exc diff --git a/integrations/ros2/intrinsic_moveit/scripts/check_plan_grasps.py b/integrations/ros2/intrinsic_moveit/scripts/check_plan_grasps.py new file mode 100755 index 0000000..ee27a1c --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/scripts/check_plan_grasps.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +import os +import sys +import time + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +import ros_env + +ros_env.ensure() + +import rclpy +from moveit_msgs.msg import MoveItErrorCodes +from moveit_planning_interfaces.srv import PlanGrasps + +ARM_JOINTS = [ + 'shoulder_pan_joint', + 'shoulder_lift_joint', + 'elbow_joint', + 'wrist_1_joint', + 'wrist_2_joint', + 'wrist_3_joint', +] + + +def main(): + rclpy.init() + node = rclpy.create_node('check_plan_grasps') + client = node.create_client(PlanGrasps, '/grasp_planning/plan_grasps') + if not client.wait_for_service(timeout_sec=120.0): + raise SystemExit('FAIL plan_grasps service unavailable') + + deadline = time.monotonic() + 180.0 + response = None + while time.monotonic() < deadline: + request = PlanGrasps.Request() + request.group_name = 'ur_manipulator' + request.end_effector_group = 'hand' + request.tool_frame = 'hande_tcp' + request.planning_timeout_sec = 10.0 + request.gripper_motion_duration_sec = 0.75 + request.retract_dist_m = 0.1 + request.surfaces = [0, 1, 2, 3, 4, 5] + request.num_rotations = 4 + request.target.id = 'raw_stock' + future = client.call_async(request) + started = time.monotonic() + while rclpy.ok() and not future.done() and time.monotonic() - started < 40.0: + rclpy.spin_once(node, timeout_sec=0.1) + if not future.done() or future.result() is None: + print('plan_grasps attempt returned no response, retrying') + time.sleep(2.0) + continue + response = future.result() + print(f'plan_grasps code={response.error_code.val} grasps={len(response.grasps)}') + if response.error_code.val == MoveItErrorCodes.SUCCESS and response.grasps: + break + time.sleep(2.0) + + if response is None: + raise SystemExit('FAIL no plan_grasps response') + if response.error_code.val != MoveItErrorCodes.SUCCESS: + raise SystemExit(f'FAIL error_code {response.error_code.val}') + if len(response.grasps) == 0: + raise SystemExit('FAIL zero grasps') + n = len(response.grasps) + if not (n == len(response.pre_grasp_poses) == len(response.grasp_ik_solutions) + == len(response.pregrasp_ik_solutions)): + raise SystemExit( + 'FAIL length mismatch ' + f'grasps={n} pre={len(response.pre_grasp_poses)} ' + f'ik={len(response.grasp_ik_solutions)} pre_ik={len(response.pregrasp_ik_solutions)}') + + grasp_ik = response.grasp_ik_solutions[0] + pre_ik = response.pregrasp_ik_solutions[0] + if list(grasp_ik.name) != ARM_JOINTS or list(pre_ik.name) != ARM_JOINTS: + raise SystemExit(f'FAIL ik names grasp={list(grasp_ik.name)} pre={list(pre_ik.name)}') + if len(grasp_ik.position) != 6 or len(pre_ik.position) != 6: + raise SystemExit('FAIL ik position length') + + grasp = response.grasps[0] + if list(grasp.grasp_posture.joint_names) != ['hande_left_finger_joint']: + raise SystemExit(f'FAIL grasp posture joints {list(grasp.grasp_posture.joint_names)}') + if list(grasp.pre_grasp_posture.joint_names) != ['hande_left_finger_joint']: + raise SystemExit('FAIL pregrasp posture joints') + pre_q = grasp.pre_grasp_posture.points[0].positions[0] + grasp_q = grasp.grasp_posture.points[0].positions[0] + if abs(pre_q - (-0.001)) > 1e-6 or abs(grasp_q - 0.025) > 1e-6: + raise SystemExit(f'FAIL postures pre={pre_q} grasp={grasp_q}') + print(f'PASS plan_grasps grasps={n} pre_q={pre_q} grasp_q={grasp_q}') + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + try: + main() + except SystemExit: + raise + except Exception as exc: + print(f'FAIL {exc}', file=sys.stderr) + raise SystemExit(1) from exc diff --git a/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh new file mode 100755 index 0000000..bd4e109 --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh @@ -0,0 +1,58 @@ +#!/usr/bin/env bash +set -eo pipefail + +source /opt/ros/jazzy/setup.bash +source /ws/install/setup.bash +mkdir -p /recordings + +if [[ "${1:-}" == "ros2" && "${2:-}" == "launch" && "${3:-}" == "intrinsic_foxglove_demo" && "${4:-}" == "demo.launch.py" ]]; then + shift 4 + child=0 + stopping=0 + forward() { + stopping=1 + if [[ "${child}" -ne 0 ]]; then + # The launch process is a session leader. Signal the whole group so + # rosbag2 sees SIGINT and writes metadata.yaml plus the MCAP summary. + kill -INT -- "-${child}" 2>/dev/null || kill -INT "${child}" 2>/dev/null || true + fi + } + trap forward INT TERM + python3 -c 'import os, sys; os.setsid(); os.execvp(sys.argv[1], sys.argv[1:])' \ + ros2 launch intrinsic_foxglove_demo demo.launch.py \ + "record:=${RECORD:-true}" \ + "cycles:=${DEMO_CYCLES:-0}" \ + "$@" & + child=$! + signal_at=0 + set +e + while true; do + if ! kill -0 "${child}" 2>/dev/null; then + break + fi + if [[ "${stopping}" -eq 1 ]]; then + if [[ "${signal_at}" -eq 0 ]]; then + signal_at=${SECONDS} + fi + if compgen -G "/recordings/intrinsic_grasp_demo_*/metadata.yaml" > /dev/null; then + break + fi + if [[ $((SECONDS - signal_at)) -ge 25 ]]; then + break + fi + fi + sleep 0.3 + done + if kill -0 "${child}" 2>/dev/null; then + kill -TERM -- "-${child}" 2>/dev/null || true + sleep 1 + fi + wait "${child}" + status=$? + # Bag finalization can rewrite metadata.yaml as it exits. Chown after that. + chown -R "$(stat -c '%u:%g' /recordings)" /recordings || true + set -e + exit "${status}" +fi + +exec "$@" diff --git a/integrations/ros2/intrinsic_moveit/scripts/ros_env.py b/integrations/ros2/intrinsic_moveit/scripts/ros_env.py new file mode 100755 index 0000000..f35db8f --- /dev/null +++ b/integrations/ros2/intrinsic_moveit/scripts/ros_env.py @@ -0,0 +1,23 @@ +import os +import sys + + +def ensure(): + if os.environ.get('DEMO_ROS_REEXEC') == '1': + return + prefix = os.environ.get('AMENT_PREFIX_PATH', '') + if '/ws/install' in prefix: + return + os.environ['DEMO_ROS_REEXEC'] = '1' + script = sys.argv[0] + args = ' '.join(_quote(arg) for arg in sys.argv[1:]) + command = ( + 'source /opt/ros/jazzy/setup.bash && ' + 'source /ws/install/setup.bash && ' + f'exec python3 {_quote(script)} {args}' + ) + os.execvp('bash', ['bash', '-c', command]) + + +def _quote(text): + return "'" + text.replace("'", "'\\''") + "'" From 6185367453403414e1af16d4178a0b94bb4355cf Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 28 Sep 2026 22:33:07 +0000 Subject: [PATCH 2/5] Address review: natural work-facing motion, quiet healthcheck, clean shutdown, layout and README polish Co-authored-by: Mateusz Sadowski --- integrations/ros2/intrinsic_moveit/Dockerfile | 46 +- integrations/ros2/intrinsic_moveit/README.md | 35 +- .../ros2/intrinsic_moveit/compose.yaml | 10 +- .../intrinsic_moveit_grasp_demo.json | 216 +++--- .../intrinsic_moveit_grasp_demo_playback.json | 668 ++++++++++++++++++ .../config/demo_params.yaml | 5 +- .../__pycache__/geometry.cpython-312.pyc | Bin 0 -> 8880 bytes .../grasp_demo_driver.cpython-312.pyc | Bin 0 -> 78479 bytes .../intrinsic_foxglove_demo/geometry.py | 7 +- .../grasp_demo_driver.py | 480 +++++++------ .../src/moveit_planning_node_standalone.cpp | 2 - .../__pycache__/check_motion.cpython-312.pyc | Bin 0 -> 5758 bytes .../intrinsic_moveit/scripts/check_motion.py | 25 +- .../intrinsic_moveit/scripts/entrypoint.sh | 36 +- 14 files changed, 1138 insertions(+), 392 deletions(-) create mode 100644 integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/geometry.cpython-312.pyc create mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/grasp_demo_driver.cpython-312.pyc create mode 100644 integrations/ros2/intrinsic_moveit/scripts/__pycache__/check_motion.cpython-312.pyc diff --git a/integrations/ros2/intrinsic_moveit/Dockerfile b/integrations/ros2/intrinsic_moveit/Dockerfile index 2376c5c..1e0c08a 100644 --- a/integrations/ros2/intrinsic_moveit/Dockerfile +++ b/integrations/ros2/intrinsic_moveit/Dockerfile @@ -1,6 +1,10 @@ # syntax=docker/dockerfile:1 ARG ROS_DISTRO=jazzy -FROM ros:${ROS_DISTRO}-ros-base +# Pin the base image that this tutorial was tested against. Jazzy apt snapshots +# on snapshots.ros.org do not include the September 2026 package set, so the +# ROS packages themselves still resolve from packages.ros.org. +ARG ROS_BASE_DIGEST=sha256:c3706ef0a0aa45413c07803cf433602f543b22e45b4855f6fca955c2d8ecc4e8 +FROM ros:${ROS_DISTRO}-ros-base@${ROS_BASE_DIGEST} ARG ROS_DISTRO ARG INTRINSIC_MOVEIT_REPO=https://github.com/intrinsic-ai/intrinsic-moveit.git ARG INTRINSIC_MOVEIT_COMMIT=c5e3290aa0f0e64c2d106a2fb4eb10cb52592205 @@ -8,24 +12,47 @@ ENV DEBIAN_FRONTEND=noninteractive \ ROS_HOME=/tmp/ros \ ROS_LOG_DIR=/tmp/ros/log \ INTRINSIC_MOVEIT_COMMIT=${INTRINSIC_MOVEIT_COMMIT} \ - ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST \ - RCUTILS_COLORIZED_OUTPUT=1 + ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST +# ur-description Depends on rviz2. Extract the share files so the URDF resolves +# without installing RViz, Qt, or LLVM. RUN apt-get update && apt-get install -y --no-install-recommends \ git \ + build-essential \ python3-numpy \ python3-websockets \ - ros-${ROS_DISTRO}-moveit \ + python3-colcon-common-extensions \ + ros-${ROS_DISTRO}-moveit-core \ + ros-${ROS_DISTRO}-moveit-ros-planning \ + ros-${ROS_DISTRO}-moveit-ros-planning-interface \ + ros-${ROS_DISTRO}-moveit-ros-move-group \ + ros-${ROS_DISTRO}-moveit-planners-ompl \ + ros-${ROS_DISTRO}-pilz-industrial-motion-planner \ + ros-${ROS_DISTRO}-moveit-kinematics \ + ros-${ROS_DISTRO}-moveit-simple-controller-manager \ + ros-${ROS_DISTRO}-moveit-configs-utils \ ros-${ROS_DISTRO}-moveit-task-constructor-core \ ros-${ROS_DISTRO}-ros2-control \ ros-${ROS_DISTRO}-ros2-controllers \ - ros-${ROS_DISTRO}-ur-description \ + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-xacro \ ros-${ROS_DISTRO}-foxglove-bridge \ ros-${ROS_DISTRO}-rosbag2-storage-mcap \ + && apt-get download ros-${ROS_DISTRO}-ur-description \ + && dpkg-deb -x ros-${ROS_DISTRO}-ur-description_*.deb / \ + && rm -f ros-${ROS_DISTRO}-ur-description_*.deb \ && rm -rf /var/lib/apt/lists/* -RUN git clone ${INTRINSIC_MOVEIT_REPO} /opt/intrinsic-moveit \ - && git -C /opt/intrinsic-moveit checkout ${INTRINSIC_MOVEIT_COMMIT} \ +# robot_hardware_description's CMakeLists find_package(rviz2)s but never links it. +# A config-only stub satisfies that check without the RViz/Qt/LLVM stack. +RUN mkdir -p /opt/ros/${ROS_DISTRO}/share/rviz2/cmake \ + /opt/ros/${ROS_DISTRO}/share/ament_index/resource_index/packages \ + && printf 'set(rviz2_FOUND TRUE)\n' > /opt/ros/${ROS_DISTRO}/share/rviz2/cmake/rviz2Config.cmake \ + && : > /opt/ros/${ROS_DISTRO}/share/ament_index/resource_index/packages/rviz2 + +RUN git init /opt/intrinsic-moveit \ + && git -C /opt/intrinsic-moveit fetch --depth 1 ${INTRINSIC_MOVEIT_REPO} ${INTRINSIC_MOVEIT_COMMIT} \ + && git -C /opt/intrinsic-moveit checkout FETCH_HEAD \ && rm -rf /opt/intrinsic-moveit/.git WORKDIR /ws @@ -57,6 +84,11 @@ RUN chmod +x /opt/demo/scripts/*.sh \ && printf '\n[ -f /opt/ros/%s/setup.bash ] && . /opt/ros/%s/setup.bash\n[ -f /ws/install/setup.bash ] && . /ws/install/setup.bash\n' \ "${ROS_DISTRO}" "${ROS_DISTRO}" >> /etc/bash.bashrc +# moveit_configs_utils ships default pipeline files for CHOMP and STOMP and +# loads every one it finds. Those plugins are not installed in this headless image. +RUN rm -f /opt/ros/${ROS_DISTRO}/share/moveit_configs_utils/default_configs/stomp_planning.yaml \ + /opt/ros/${ROS_DISTRO}/share/moveit_configs_utils/default_configs/chomp_planning.yaml + EXPOSE 8765 ENTRYPOINT ["/opt/demo/scripts/entrypoint.sh"] CMD ["ros2", "launch", "intrinsic_foxglove_demo", "demo.launch.py"] diff --git a/integrations/ros2/intrinsic_moveit/README.md b/integrations/ros2/intrinsic_moveit/README.md index f48a797..1130fe6 100644 --- a/integrations/ros2/intrinsic_moveit/README.md +++ b/integrations/ros2/intrinsic_moveit/README.md @@ -55,22 +55,27 @@ flowchart LR ## Requirements - Docker with Compose v2 -- About 6 GB of free disk (the image is about 3 GB) +- About 8 GB of free disk. The built image is 3.1 GB - Tested on x86_64. Jazzy publishes arm64 builds of these packages, but that path is untested - [Foxglove](https://foxglove.dev) desktop or [app.foxglove.dev](https://app.foxglove.dev) No display server or GPU is required. The container launches MoveIt with `headless:=true`. -Packages used to build the image on Ubuntu Noble (versions float with the Jazzy apt snapshot; recorded September 2026): +The base image is pinned to `ros:jazzy-ros-base@sha256:c3706ef0a0aa45413c07803cf433602f543b22e45b4855f6fca955c2d8ecc4e8`. Apt packages are not pinned: `snapshots.ros.org` has Jazzy indexes, but none for the September 2026 set this tutorial was tested with (the newest indexed dates are from 2024), so the build follows `packages.ros.org`. Versions observed in the image: | Package | Version observed in this image | | --- | --- | -| `ros-jazzy-moveit` | 2.12.4-1noble.20260905.083030 | +| `ros-jazzy-moveit-core` | 2.12.4-1noble.20260903.075716 | +| `ros-jazzy-moveit-ros-move-group` | 2.12.4-1noble.20260903.094420 | +| `ros-jazzy-moveit-planners-ompl` | 2.12.4-1noble.20260903.093406 | +| `ros-jazzy-pilz-industrial-motion-planner` | 2.12.4-1noble.20260903.100010 | | `ros-jazzy-moveit-task-constructor-core` | 0.1.8-1noble.20260904.024044 | | `ros-jazzy-foxglove-bridge` | 3.5.0-1noble.20260902.084741 | | `ros-jazzy-rosbag2-storage-mcap` | 0.26.11-1noble.20260903.070458 | | `ros-jazzy-ur-description` | 3.5.1-1noble.20260905.072414 | +The image installs the MoveIt libraries the launch file and standalone node link, not the `ros-jazzy-moveit` metapackage. `ur-description` is unpacked from its deb without installing the package, because that package depends on `rviz2`. The URDF's `find_package(rviz2)` is satisfied by an empty CMake config so RViz, Qt, and LLVM are not installed. Default CHOMP and STOMP pipeline files shipped by `moveit_configs_utils` are removed; joint transits use Pilz PTP and fall back to OMPL. + ## Quick start ```bash @@ -108,10 +113,10 @@ The arm stands on a grey table. The shaded rectangle is the OMTS return-shift wi | Phase | What happens | | --- | --- | | `SPAWN_WORKPIECE` | The billet is added to the MoveIt scene | -| `PLAN_GRASPS` | `/grasp_planning/plan_grasps` runs the MTC pipeline. Distinct candidates are drawn on the billet | -| `SELECT_GRASP` | The best feasible candidate turns green. The Raw Messages panel shows the `moveit_msgs/Grasp` | +| `PLAN_GRASPS` | `/grasp_planning/plan_grasps` runs the MTC pipeline. The planner returns 10 IK variants of typically two distinct poses, and those poses are drawn on the billet | +| `SELECT_GRASP` | The closest feasible candidate turns green. The Raw Messages panel shows the `moveit_msgs/Grasp` | | `OPEN_GRIPPER` | The Hand-E opens | -| `MOVE_TO_PREGRASP` | OMPL plans to Intrinsic's pre-grasp IK solution and the arm moves | +| `MOVE_TO_PREGRASP` | The driver picks the feasible IK solution closest to the current joints (each joint wrapped by `2π` into the UR limits) and plans a joint transit to it | | `APPROACH` | A 10 cm Cartesian move along the tool to the grasp pose | | `GRASP` | The fingers close to the billet width and the object is attached to `hande_tcp` | | `RETREAT` | 10 cm back along the tool | @@ -119,11 +124,13 @@ The arm stands on a grey table. The shaded rectangle is the OMTS return-shift wi | `PLACE_DESCEND` | Cartesian move down onto the ghost | | `RELEASE` | The gripper opens and the billet is detached | | `RETREAT_UP` | The tool backs off | -| `PARK` | The gripper closes and the arm returns to the SRDF `ready` pose. The cycle counter increments | +| `PARK` | The gripper closes and the arm returns to the work-facing home pose. The cycle counter increments | + +Home is the SRDF `ready` pose with `shoulder_pan_joint` rotated by π (`-2.8173` instead of `-0.1597`), so `hande_tcp` sits above the OMTS window and points down. Joint transits (home, pre-grasp, and place) request Pilz PTP and fall back to OMPL if that pipeline rejects the goal. -The loop then plans a new grasp for the billet at its new pose. Candidates are ranked by `grasp_quality` (highest first), with a more top-down approach winning ties. Up to three distinct poses are tried if a pre-grasp motion fails. +The loop then plans a new grasp for the billet at its new pose. Feasible IK variants are ranked by weighted joint distance from the current arm, after wrapping each joint onto the equivalent angle closest to where it is now. `grasp_quality` only breaks ties. Variants that would flip `wrist_2` by more than π/2, or swing the base more than π/2 away from home, are dropped. Up to three distinct poses are tried if a pre-grasp motion fails. If none of Intrinsic's IK solutions pass, the driver calls `/compute_ik` on the pre-grasp pose seeded with the current joints. -The billet is 50.8 mm across the gripped face and the Hand-E stroke is about 50 mm (`open = -0.001`, `closed = 0.025` on `hande_left_finger_joint`). The fingers barely move at the moment of grasp. The hand is opened before the approach and parked closed between cycles so the motion is visible. +The billet is 50.8 mm across the gripped face. `gripper_max_opening` is 0.052 m because the upstream open posture is `-0.001` m per finger on a 0.050 m nominal stroke (`opening = 0.050 - 2q`). The computed close command for this face is about `-0.0004` m, so the fingers move about 0.6 mm at the moment of grasp. The hand is opened before the approach and parked closed between cycles, and that open/close is the motion you see. ### Topics @@ -144,13 +151,13 @@ The billet is 50.8 mm across the gripped face and the Hand-E stroke is about 50 Also published by the upstream launch: `/robot_description`, `/robot_description_semantic`, `/tf`, `/tf_static`, `/joint_states`, `/ur_manipulator_controller/controller_state`, and `/rosout`. -The layout's **Details** tab plots commanded and actual arm joints from the joint trajectory controller, the gripper joint (`/joint_states.position[1]`, `hande_left_finger_joint`), and grasp-planning latency. +The layout's **Details** tab plots arm-joint feedback, the gripper joint (`/joint_states.position[1]`, `hande_left_finger_joint`), grasp-planning latency and candidate count, and the cycle and failure counters. ## Recordings -With `RECORD=true` (the default), rosbag2 writes an MCAP file under `./recordings/intrinsic_grasp_demo_/`. Open that file in Foxglove, import the same layout, and enable the **URDF (offline web)** layer (topic `/robot_description_web`) while hiding the live `/robot_description` layer. Mesh URLs then load from `raw.githubusercontent.com` at the pinned commit, which sends `access-control-allow-origin: *`. +With `RECORD=true` (the default), rosbag2 writes an MCAP file under `./recordings/intrinsic_grasp_demo_/`. Open that file in Foxglove and import [`foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json`](foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json). It matches the live layout, except the **URDF (offline web)** layer (`/robot_description_web`) is on and the live `/robot_description` layer is off. Mesh URLs then load from `raw.githubusercontent.com` at the pinned commit, which sends `access-control-allow-origin: *`. -Live Foxglove sessions should keep the `/robot_description` URDF layer enabled. The bridge fetches `package://` meshes itself. The Hand-E body DAE is about 12 MB, so the first load is slow. +Live Foxglove sessions should import [`foxglove_layouts/intrinsic_moveit_grasp_demo.json`](foxglove_layouts/intrinsic_moveit_grasp_demo.json) and keep the `/robot_description` URDF layer enabled. The bridge fetches `package://` meshes itself. The Hand-E body DAE is about 12 MB, so the first load is slow. ## Call the grasp service yourself @@ -211,9 +218,9 @@ The standalone node only compiles the SDK-free sources. A commit that changes th - **Port 8765 is in use.** Stop the other process or change the host mapping in `compose.yaml`. - **The robot has no meshes.** Wait for the first asset fetch (tens of megabytes). Confirm the bridge is the Foxglove WebSocket endpoint, not a raw rosbridge URL. The subprotocol is `foxglove.sdk.v1`. - **Offline playback has no meshes.** Toggle the URDF layer to `/robot_description_web`. -- **The arm pauses in `RECOVER`.** A sampled place pose was unreachable. The driver detaches, returns to ready, and samples a new billet pose. `/demo/failures` counts these events. +- **The arm pauses in `RECOVER`.** A sampled place pose was unreachable. The driver detaches, returns to the work-facing home pose, and samples a new billet pose. `/demo/failures` counts these events. - **Logs.** `docker compose logs -f`. ## License -`intrinsic-moveit` is Apache-2.0. `robot_hardware_moveit_config` is BSD-3-Clause. The standalone `moveit_planning_node` is derived from Intrinsic's `moveit_planning_node.cpp` and stays under Apache-2.0. The demo driver, launch file, and layout in this folder are Apache-2.0. +`intrinsic-moveit` is Apache-2.0. `robot_hardware_moveit_config` is BSD-3-Clause. The standalone `moveit_planning_node` is derived from Intrinsic's `moveit_planning_node.cpp` and keeps that file's Apache-2.0 header and the "derived from" notice. diff --git a/integrations/ros2/intrinsic_moveit/compose.yaml b/integrations/ros2/intrinsic_moveit/compose.yaml index 2e380c0..c4b6659 100644 --- a/integrations/ros2/intrinsic_moveit/compose.yaml +++ b/integrations/ros2/intrinsic_moveit/compose.yaml @@ -17,8 +17,8 @@ services: stop_signal: SIGINT stop_grace_period: 30s healthcheck: - test: ["CMD-SHELL", "bash -c 'G7T(?~k06HWQpi{B~bV&|?Rgx2+TXF&PNL2u@2|3EKoeAtcOp1ICmzA_kKrNybHvOWKk)%*UZN$tQWUEF=Y^d`<6Z zLQIO1?G@;aIwih*tXKo|CyoPm(4Us`dM;>ruTV~(fDyGcU^hyoToNSF;H#V`#t$qU zV2Q>xU^OnG7O@P&<^b(Y{t?s1TeN~9St}gRkVlye%Eq&4k^!m}-KB<`km$ijuR`?0hXIP0N-*4fM+-YDv{*Qc~J>U%bz610)B_g z5W;=aCvaojkN6Q1ASVJu6?+Hc;RI-&t!=+bF331qOUuWJeIs%DihMrY6WP}vOGNr; zIME-E$@^$r?!X$_ACvogLecnzNPhxrLnHD&nf8?7dgG&ggMbf7k!XC)`o2g!8cEQx zw)0~jyAJn7Vv*7F^zrfLq2X{Ml!%9-;Y8cv!FW%2P=374#G`;@KY+h-|4eZo`U2UZ zt2?t~X1C%Cd?ce}F4i@WKHuN=PR`B@eL5ig1*%x09>^Hbt8PC*tD(vu_{XD+$la@2#Z0mF+8_u=PMy8{RcZcHI zk?YY$nm={J_s*HQbIPvAZk+yk=Lh8dqaSoGJa_7sN8dlLeEM?>&ePCEhGaUY#VpBo z&Wh93imN%tTeMeN?y-xZeEVC&`Jwstx#71al$JyDvK0%;!^p*Lhfrt#hm)(>_z#!h z@!#eI(Z390=2mPOTp4CTe*tiYD{T`?E6F8LkyyG;jC}~3%d}r*?JX=>Y3mtVa02!_ z*w7#-;Ij#r(f0ra_6UnLL zTopJw;Hq?-y8`FPkXRewJvzP`bH;6?thUCi?f&FBNW9UC`4=Lw{urM`V!dFwa)!`m zt$CrP)@F}0WZXl}W}|s9HhY}m%GsloG|4z(KLae|1M+*DiN?JqaVDX4%w#1Wr4_=} zTZ@0uoF&eLtu5qCCS|j9#J;!=>1$G6&9k`@7Sdl!JCNs<^C`&{*JCYC?dE(OrJa)$ zjgiah>=tzni#oxm>lMH&dQVXBd74RQmYmkW&p!AdEj$sh-9n#s3&WUzjc$XCHfv?9 zH_BK=v0_fA(aVem8$mrQm(yT9^vT+?KJ9cPRH=@zTyhkun4sWBsShx81GaFgP8v@z zhpG}vcVZJ!1`ijZxVLCMf(8VQ0A%!anmcJ}(H#o7CG?HxS7-pxbx3y-SW9tB&Z^5* zQ&pKq-ad5G8Ax?5d3;yzo7p|RJKKKc!Gfnbb+qUJn}6{uOSS$P*R(6UGq-*A!9wk> z{6?jAPg*Q`YBPtXo=uZtbxk@k^@Yq3c#{#Ox;eK~@!WrJ)vuAFt15kPsv*;!d4Brg z4_!^*mHe|K;FS)}HGH^jzrn-y9Leo?%Q3q*-j}pSPkwcQKu0}H|dx&;)g2R){d(DyM9`c)|0&4Ws2@HxSk?Q_%Sm(`h+Wpy@lpF>=Z zN3U;y_w2PDhLMx}X>N``aSI*vE#xP_Yg>M0DQA;i6UG`s!Ev4VSBD!5`Xi}hPz<)q1z#fHt9h+eF3G71KAjnyOYV?)CS zB*}oFqsp?7Nx!0{Z5=Jm-d42a*U=KyhouBwqp$?ra*_bvn!(-`4R2$60 z!s3+X0e5`X6ehn(IG0%VVe*xP(}^X8X=#&SGv-9Nf7L>KR4*uFbWim%3-Ph6Rk$ch zn&}zEIHA&-U`$TYIWfBe?hkz#eawOq1-$CQs76;#2=SQ}dJ>4ffP_B)0BY&>zWxw+ zv--{1r*qQm$wK{}RPfc~lgBTeTz8#sOZJO7nkVpeiGi zTvrbrKRfyCr4z-w7TqkwwI7>9-2mXr{t+s0u@%@)7|qlV*BfIq>E7mL#M;}VO$Rq}j5&@2=# zwdyT94uvj6dT0=8D*|zQf9yhpCg{_E8bUOJ6j$^%W;bM8;rm=R3}3zCZAo<%>$l}> zIUjtTIZ>(Kl{&dp<+;3TYFEbhdLVVAxS?t0f$0ZQN7A;m556bUr{J?sKBFTto3me2 ze4rINzB4UO9xIks$aH$Z0tS<8g&qP);|dKxMP;(S?6sr>SyrYs`b7Iyk=CFz-A;+T zPk^=OH5acvvdOh&zjwn1Y=qy;9qW&9fDdw)g&tcU*JC>enh96Jf}5MRebXUu>AH2&8t_KNgPTQX?!>)v2XywQuOtz2fR1De)aT6;q^+b zX3s$8PXT|b;)U?wa70GS07h5`%mX7)0;=!PD5a3hF!KXeD7Q4*xOa(pVD zO%|McmeyO7X`kxLOcb0uS&Mc3y6ts(?6*0mX#eOA&(%J_BBS>YR0Zon(Ln%4!T_Ib z^DZd7%DqbPHN0eI;VF#kg}b6l&Qdfh@kZ=idNDU@@a>fyFZ5gEKRmJG)d)8%UI{J7 z0DRkKKDF*y`P5n(YYGf@QaAs`7)D-31<@~Rj)6fg^heq@c%&ulkyg81#=i#e;g___ z6EN1ukt+7bq{yf$Nc~ZnIceQSr&8=%i{cI*zaPzXKin;cUJeiTh9aX0I00KcsJ;fo zGCm~#Q+rYMhdwxS_4FHc+0i`7AIdwGZS93k9fi6BKZ)E3{=DwpX9^E>{kpD8@rShM z)~|OQB6_e4mpj8z*bd=XUj)Kv7CxN8?r{ZQd{Mh|^IP#n&jQ61lC}W&2-MwOb9LiQ zf9U4tzqIhexx(ka4CR6^q_{&1LWuci7@!oE`?PB&F*Y1MKc?LW`i%GGP*m=d+W=^x zJDM+cE1<F z1_Xza((XRbz7k)|y!X7<|Ns84_b)Rt(hYFi|Nf5$`~S>f_tDmw%0ZBYRKtr?Fp?-^6~+ehd4x`mOj)8?p`C{dObeF%3C})BWk}**xSNcKKby z8UBpnOn)W|vkbY1J$?^+whm)@ zZK!Ct*k3$c;x8F4^_LEp`OAjO{pBpqKI9v&@K>;B$57>PmA{HTrw>&R*Z6DLvva6+ zxXxe4o?S!BhU@+H>^Wm-`S1$=3ig~i)G*xWZ)DHzp{C(xfAesQzh!u(ejgSwn5ZtNp9lbN0}h;kEv?!|VL(hS&SovoP<_hT)C=jl-M#n}*x{?JO*3X!CG~ zzk@yJ4s99U>fbv2nE$ciZT@Y;o&L_@E`QhXcK`O_9sV7|JN-L{clmb>@AmIz>GOv6 z4Da>t9p2~PhxdGc_h|!H@Hc|60YDCtN#F395f96jxw~`@3_Ta@E=0H zQsg_#mHB(P@-G?uM}mj?jhydGM*rh!h8+g3;!6guGPvSRlbq9kl&eCjCxXYg>a)uX z2Hu`#2tM&z0@F;(=-Y(^D z4TpRC&IGw_qeDXjA=I<~6vayJ)BxwU(UDM??;RKkha}4mzBe=`xp$A^Rf~{3`$nG* z?h1GD{3u$<1w&HCfuY`!k%5uZhx>vfK`H&v=&8{#8Wj%KTcqr*V`D=X7pCy+2!^-y z^5I};pm*dzZ}^OqfslQp)CZK3z2aL3C}8`Z1+TmI;5*YP6>DDzZ)Pe2zpkx)lwsS@ zKyW0C_q;c$u0*Kwei z?;Q??gFHsSL*sL5U}zwG@xbWNK;K0v{ov@~1N>8#h}KER+J zMmFvMKQN3hdOC|B3;ke71Q;eCuB?dAG< zLuibYrH1YuK)?vvXOx`#dik?Kp3RZ^bjdm}@^p|7OF6^6DD*<0pC26#T(~$M_}sbP zu#}ky4UY!KFqJJ1jpsC^dG%5eoxq%@r(1)2lz+rx%RBCxh^2!k*nm}scvcA>9 zRjun=dV5>?TY_ua`dYb_Eo*vP`%kq6Ppxd}JGHuX_4?MnR^yZ{XTlgxY)uA3#PDV!K4J_2bjePO4=aET zDI>-~vywvl9S2b4jI=c>B`LosQB5B)9yZjcVb6sN{`FsPt+(*h2FXN2V+sZPBnwvK z2q)P_dPgwb`Y=Ci_!%AHLXw?JY>6Riq0e`JqWSzNe>ODM+ZUw%2%eUA%uq8w8fs1I zp z`s~hGbIzR8`|SRE$ZY6S2L&(G?Q9VL48sF_!WgC`%nho;q=Xm@)cq4CE^We$KT8;I zvP&91W5hs16)}xF7Z25hHSAFGM68^NrL=P9aRk1ZsISv-Vhw7sVItU23hJQP346pI zNpl)F3*|YBv>_IPcf_9U1iUj|E)Anj*&(%&QYw>V!Vxk5iZ$YhSXf@;$wipaVQu*$B+2Jz5MB5*tf#h`u%6m`e-3@ zLEpFD9wKPq3!mu?`vyY3z9B4u5Z*_9edl-{+hr5qjxxG`Ok?q5nixNN@xTA!)X)BW zW^=tmGGjYFBc+WE@Y@i>cfrGQ>czGqnR-tFc=v{TM_RGW21X=P-)N}bDOu%qNVY`z zl7+PZ;pYJg1|;(-)Gs;64{+EOB`e##&Przb3d!_vBvbpZwg_!KvEml{c2hJ&m6jY?jifdCrqJnKNa* zUMhHMqn0_R=StU=HItoJ+wVAiQ(evHyPl2AdvdO&Urm3d?bUVH*G+AoJ}OkV#fw+J z6aJ$MZ(q1&y`3*?IS^laFzz{odfj=GxtE?{#a^j-Vbh$;b7lLb?r7&-tHI@YcF$dl zAv62Rkt^F@IH5+-SMTBXpilbnr#c#Ja`%1>9rZl6fuMno68^NP0SKq@OLiP2(tui; zITL&fXNGU(Ebwid6~3Lb!FOBGdLIgOfCbyo6Cgn;o3Pj(AX@_ zgSTuh3%-}jhM&WE;pcKW@bkD_`1xEO`~of?ej!%?zlbY@U(6N3FX4*emvSZW%eYeb zXdt_FS`R||g`R|mhITLynQR}X&$ zw;X;0w*r16*8snXYlPp-HNkJ;n&GeHTHpgifWL}sh2O@lg1?$;gTIDb4Sy}y!L31C z)^Tg`ww_xDe*?E3{zh&C{7u|O`0d;#_?x4K3r2qjVAvMP%}~uw0N+jq5f%Vnb>F^% z{q(CwfM0nC;a^IN7+%D#$r+Ws&uGwg>NgYVB8C7hZ|1Z;c@R5hs!Pg2AfLGbfa!n= zvXk=b>~=}{)xDoX8v|-x?9Hi^9WGR!28)y@?UyqtUxgCXo%ol?<5tSz=o{4bvbU@S zaoNh-X-er`@Rp;zv3$Aen;9wd7QA8SmR-a!xKer6ewE%aqK?5;TDbD0#fMibVVorl z2r0Y4b;^_W8{VKi4{lPPwBNU^%879TuzHYSMv4n>Q35zy#HggkmsKd?56uVYo0PBe zA^El|@tk8pzG@|WiTZXc@f;v$Qhl{b_(StK7v!sZNWKF~Jm=yvlIp8h!k4H|%ZF_+ zDc|yk~bGb+{qP%IpTn56`T}}&sEAPz&uFKY`UfJ4Q z%tskxT3+P|xZOj`4u_P~%DTs@Q&)5Nyb_C&v<+Vg#15ihlE|g5{P5?M1WAW5LtwKO zI{1w8to?EYsN-4fU3psa8xqij^^ab8g~<7wQi}HbNag*imR@-Z|C;i|6+NsB^ajoc zvO`+E(q2&Fx#CACZM%|siPHXEQv4O=S^Is28VIRnZiyPcqQrA0k5JkkCG`@eO)BwR z=_8c3Pf5K*X|E{pT-hU(wqHpdzM(ut4B_8Vp1JZR%P`AnFuI47)CEEM6Hk#K1O8GAQuStje#l^J~Qr06au2zIZDM7 zp%kQsP#=iCjE;6bcxpT=xl|&*g?^llMvmvm;bS}zKR}{jQA%hiI0TX|Dr0fu1&Nv# zga(E~r^lUgOy9-6q2PF?{Lp>990<7Qf z7dNN0jN;`l4&Bdh?i(E*1MwSeO^}vZuitm75d&wFUi#18_cjM*dU*iE%LKvven~Tt z>H?rcg?SLngFGr_B>6z!FjxP$WC@)EeO|H$0s|uh;Xr^VBv~>A`c6rXFrz4=!IF9J zXx~}M5*VT%n@mB(vApYP9ET+r$b%q5v2F^1w$z&-&`Zuh7}U~{-cv&$@TZN8Nv7b% zAd>e4K>Y&YboA7ql#Vx=QS_V<7z=`wiVmX5fb6HwjN<0yFAcT;M zA<2r*9RS_l5m080WMT6`vMVz|@?u){4fXOt^kIS$9+EuxoL(*|gfGLl^X1e5_CsBJ zcWr%aZx>%mukM~hTe}bM>gw(Z?A^a@>t4yz2f8H&XYAZ5Fd@zac^-@rgXEA`6hfs8 zbp=r(p0ITgqOoq+D2rJe$b9(@;_foA(@mSiA~^2)Epid##D7u z`>4|~yQEC@h4nmoT^^S{#x>#T+$A~X)yER>bcT|gNh^(~^B4FAj9tT)DCQCW^gtgb zU{WAYhc@(_vKVxiTeTTu z)Gyi4)5O%mEDm6zhC+crh!7v2?^#BN;-ALLc)vRN*>U&`^ zDKHof)v*rtQA!~S^TldSmC3bC|RC~cj0 z<;MzF&aAy>GL~&(&HJdJL@a0&3L4+yVr_eGpN1{){2#D zh03+xbi}qDi#>TFUU`xt%I7L-#ELaS#hQ0MAM5nTj-QNI1Sq0x&Q~q^Rtvt>?*wD* zM`OpHjQdy}rRb`}$DNxQzI7;GxRZUF+N7G;`dznM<0aj4i?r0SHBZJ)1mcBHvGm2| zVsVR5-11I-Yy+#enAKaLcKW<4Z_+)TBi3&g>Nnp?iyb~H9)40d{N!De!BO$4!RR<) zOiG&_%UL#EEiUgAmUrGdjlm`&BB+3rG|f@-sohZIdzpqhbH2D?y65XB-#B@*Ctmy* zHL_s7taYX>Ubc>2irGjP3iiUu0_t&xLy&utcH44Rz^RBX(Z|!aK_w66p(d3e*dj@04AtS3VDN$amVD&BTduQH1 zqb54AC{f{Ln^?R`C|-4|^Lsnr->IhTPNbwhULh2&n0J-L%CtI*8c=5u)p20}zrTuKe*e`5^3tC59^CtxDTU%yD>v*>V$u5MrN@}FMvN~T zemm_Ubv<|-R+2GgaSyQ zLq?&c^zi8D5IFmY=YJ1vmx>nQ4gtcCh=n8NYYb$WJqqM@7|f@7msHF%9#tT8hB)M> zh#wmYTnqPpF@ZM$$A6`w-`Ma2JG*XiWL+&0sj?dDT~ZR z9egjok_rh7O9f0)0I2iRL14!assMK13tsqXsYpc@7RR}iDn^0wN}0+VV>WS8p7K62 zibg~30AehoXCd97gb(*#2vAO{reLi19Qg95g5mQJ5DAQ(K_`W%sr5F=nHUCcU^pb@ zCSDjLj0rwKjN&{xpAhswXkZ+;x%>>d1rpEjF6hogVBUhjL?2(cC>7HAVE}DS)NCI*1LGfmlXk%Nw5%!O4p5L%_P`y=!L`zcCNV_B2ew#Y&nb`q8Q6((6Yh4((H6606IV6r=XZL+i>=ay_Ij6~cw-pvv4} zweqC>4yue7Ei9}uRhIbP%tvX1k_O+cGU=8m&#k1@`zGY4BPnxBLFIHBUNZHi zoi_BPoggt8lQyR!>aKG&?Xug96cZ-2pWv0;#}Au*c@L^ri9?A*my$~)8Y6RPmANl% zfLgU&7T$SiZ{ls#gJMCFr9eAp^1 zf5Z~ieSV3S`5sgbX5TWUMN8zbcu@We#*h`FFemul@BUq!pc=etWO*G?1**n zh|(VIH)46KTFF0Rj9Ag?6-q4XJElBqzY6PU@JTI9dE#o2M}?wFD1>`zUp8}fh*L!b zv^eY(DwYJXf9x|xw!fWMp3LVB^~<^!$P*D_L=@J0JUo&ox|F7O;MF+DdRq`SHe=Fu zYt*EUAZ3v_KC9a;1a%V)GoClRVeH27BLfnP8l9gHped52pJdkSP5j^BjRmn&zyhkP zWaCCbYX&Q6<)TAC3h$t46Hb4NUi!K@;)H%FW}XiegvO6{$u>e z-e1zwUy=9MPM-<;#shur z_f1W0{o`&Q&h@?{hdQ_W=veBjZ{q)!ilZS)?E}CxgJeC#r~rH`y_irCuXccxkvN(r zPO8eQd`9K}j&fuWHGl;$nn0#XA>Zl9LPe_d>lQz>l2fx3Nm;X{p|n zT*>5tq;8$3tj?r^t>jh+2MWC)OQRPgg7kV{a>?gq@JvATkR0;qAIE)}a*}aiS9f<; zXJFg@efxIxNLJFQfu4k2C7-0zS{NEV4Uv7xJTTHfT5n?$pZ|Mmp_%9ca__Ts@n2aW z4Nb^~$ERfRSow7PA_B&J4_g>CAw&OYV(Z}3G^3-N4y8oIC#vf7o_OIpv2e3cxS1%a zjQHLplr&9$AzrdsEa?p(AX2NKN9y6Tf)xR5)DF8!}RfZ(FU<-i%_(M8qU}ZZ9-*RZ1t{q2_UDJZ;HFxMb|dL zwGE7uhAxnoXYI>AE~%KR7AxgiyW*AG#Y(6E?2VV~qv*o9m8->-+k}+6{}|U$16I;irqrR?s(}QDx>J*BY}T5 zrIn=5?K>Fl{7TKFSu9vC6fB=UezW>!UTnpdc)?b&V24n!Bc8oeaP9nr(o)8PPpt-D z%~X$AvsS2C8(ViMUUOKiIVRK`iv(T_P)^Rl6@PycKQfN3CuVsvus`;vA#9i4a)NcfnDqep`tbanNeM%<5;Lix&A>)v?zKC%3;P<}XG*n?@1 zIagJuOv*j+s=Z>>A))FJD|Id_SInvsvZ`X$?eVP5VpgY+)p;lD$X{hdt#fX#=&lgl z6|u_Aad(I4?h@Qxknmo!t}A9nGQ{zoaq2xrv3f)Tg@@op`U`_(_{SXlPW{uQT&)OKp`S7d{C8rxXSz& zP0fdw;Z%YE#Suz?{lIYVU*KIM@l+)y>36jO7!{>rPG55ep;T6iQ>ZeaedCOD95qki z45o<^YbP|+oSu>yuaUG(@GqB}4GvOe#XqG)CZeO#O|+bYDt$u>Q*bZBld23W4I$%s zU`xmFRp5?PUoagroHz0xA$`4_AIA@1*}xdXMVPu8(-mYOi~l@g`3aUJ6b#p!Wn!fX z9DNye`Co`)cmwUt^3MD!f^>G~6fXTQC}Tf7ASfAGFZnO|e*&`N>iNo=sXiL%8PgxR z-gd<++d=!wSba6~iuuZ!`I_a^>%JDaV!r0O>YD7iqgsxNkPzsO^PDmbC?uHy-6{sP6(txA0f+xor_`Z0DY&#SNhB}_ z@*%#I+{@6H*8c{?n15R(KhjjHw{(x!KyB=7@9E%%=7i+YnO>edPr@xpvRBRHl?gG4 zAm>{wp+xKy-?H%>->Hj`YHIMw$eN6U-8a^gP7X90dnFqmWO6tB&(To6n!K;VlWd8? z`OhfmJ_VVe8@e)^?rbyJ9s*fOSXX!n2Y`Wd;q#Zm)8G{+%T zyB~c5o1Hv_Ggshr)R$bIOUr{sF|N7Y*XTLTauV{-}=Tor_9W;jsPz5H+{c_| z{WZW$)MGEjm_hEFQltU%hs2Fzl~!el$^Gx-}x zBYB2KN6(%cBZ*X58J|%fpxoIv1U+OkaoxejCEuYXSxGtV3Wu1d4M{Rykc<~41vahy;8_tIa7DD?Y)k7JN~Nc`@28b{r&wP?2qp~5zjsuwapb(h(#-eq7`D%TA^rd z)IFD9Cgv{_@|TJED~0@(QP;e?YA&Nh%qSBw%BI|+Z>8W{InyDo>J(OW##~)<8HF#M zymWH%_^T(cpPW9<I;eD~mRosJmew<-anwCw9!Yx! z3gOyjtHe)OkY7cll-!yq{h+#VbmCNQ-Qu{2MQe zMz}%^RcWb+SHA`&R4Xa8Up+o%WC3Eh=#24)GPY1<2&?U{Ht;4ogIqM6S8#~}O!N92 zhD-3zpg#T!hiBqWHcKo;CIGI}05*D$DoWoJDD zTMi&A5eyn0gZMQG^gtSj5%M56e}=4OsDTYsei_Q=zea^-gdiByqihrPR{5A?SB1$P zn#{N@AuulIu0|0IUeNhRIRX7HiX1Oqw7fORVX8RvPe_%7>R24LbKZQ>TXn}<1zqf) zSrBm7K%Ui5P;%`HSHBS52>>V-)C&dm(Oq+SMb}2Jjz)L<%!xEVaT#1WchgK6`JiGY zWNK$@Ldn{zj=Oe4VP$mJe0F6#yBZrsdDW|5xc-HBd9zr)N+@3yFJCQ|ZxTR)Dc>Bm zy_9(=lS!4sKre-=#Gh!d7wq-ZbsySWe^#Tk`WK&O z8*)nN@Rt8WPab4EGVfsDEB%HF&AB5yRX#ZeGLUq40B^W@|Z>Cxq$&VuRkkfwVe+_!;Ay38a+mnK}S<5LAm)ryUk`=?QNnP6li>yXDpV zt0;qSBX2pau@&SskjFR+jH$rb3x7#T{))UGllT9Uca=P_w7_zZm!XAeV!#dq8E#6j zND{P$e~+}|WlOF!CUp;kS%%PSNSL(PjEt3!iD?Mtt zTL7t9T4pk3Yx3NO_L`rSqW-%$(bU{$b|7BN*p_DccA90IbBROE$M~A1)`A`as?B+g z9Dy04Lv~P(5^t%jEhU7J91^n!)vNB8s845AD5dMwcacy!oGi^W7i|1*p-MI{_$qqZ zM&1kLy-eP(ljowL#$VzL%D;`r@ros)xmk?+*?)C+}8)+7<{vN zwtT~^ePhbO{CD!&+p zWvXQPMX1VvV#qYtprE%H8fuX{srxseEV=uW3BQx!S$lPYaw6Kx1$+5x*6Ge!-^y8g zD;o=v82r%glY2e6XH>kDXh+XmpSL9w0rW6Y<;!2N4j5jrMXga=e;Qc{fq@#M9%(?^ z6@d|2^hO|};#fw*C957h(Fl5>5>2{K|Ck1H9FF^(2^?~>#tDDU$XPDgz_*9bS%?_$ zoDok}dV;+T+d_Ugbr9o+e+ds?O}hpnGp4}up^6XXV}=cbb> za;H)5V*tPt#xEEzSIfWW!5f!>#v7(?$qH>r@Hiz4^w>uEH%$CiGzhlX;DKBX+5-59 z=U_Jn=6He^`al{Sg<(AFnNhM(mN;HLE9)siIS}T+pqw>cyJ*dm-lWDRPoz7D@$qaQazXI;fO~;Bz4Ow}7kB^sQ%eG&shz8=LHzmObpL|z zn%evK2!wypTW5IP(P5ggY%u+?rNcCy<)Z^8=L7Z8*V_+~W*@lDEBhs5h<}Vejy@CH z+6Cn0Bk-hjYGa@`baA9l%If8~0DUPmFlB{L$$Y*SELRtuS5YLh83?gu{%I!J4nqDn zs0^#Ddq$+w&z&P9L^f&!((|;+c-ras?d0u%$H-@dw)%WqwlLy7v@EH<@m$^hgXu>@ z=!>Z4GqTlH@X|Avo{77mtTmVAjaq+dwm9mot8!{>JZlA*KXyPm(zTk( zpS9P674*`@OBW~eXWbR^*=19;uQ%Ulj#aeATDRTWaeLdX_Sk`gvGPOl?87nF;W=ma zm43kqoveb%{Hx&hRLr?^ubdIw zdWYVsPg?0%APNCPfrKGvSUu5cW?`_-&6k{PojW;mVx2qX6+dBx%#IFjiIUWl9cNX_ zf6ka>#w!_4VO+_n`(QeH#1&1Cy3ivw?1MJc@_@4~nKIMe1yw0x?1Jff{gJc_CV#ss zW6ZwY#lGF8e!GivP%r3V#Mt-J|1aP7>S^C2-Dv1Uv&~=|<}64{Y<+g&S+e)`(Xltk z$t#t1Zm44>_Bp6ICt}AN7p+t}$_7r4_9kz4N^*`?@WVE|+?V!V+WTVn12#N<7s~k& z%@%{9ZOeF#w$afZ1-%d-BtLxA2A^Cr>AT1udK5XwGguTKB!!(Ix8S_@e;tV8x1h!R z;_;Co4QZ6f@GGzcM3Q62S3J@<$%!w}D0QH36JV)vZh6bs8{cSrv-wKK4?MLDE;Zg~!!%y;fYFiblR=R)2^6{2NhHG~p0<9z8=ho>QV_qNUZ0>> z*C8lv1H{Pi$V0@LM1L?qCKsK-fD=#PRx;Dr^M|Q`WAyH1Rml*Zr`f@uq+q326A@Gw zf1Er5T>J@mA=)Mhb}_dFP6lD=z&jHwK!?Vv#yv$v{4+Uo>+-83jLN&_a3&bn(@&5y;ix5ZrBsODk8Sv8kkK4lZKm&IJm=G-Nd zPYCW>(cLJx8)Npy#pe{2Wd`-hB~O=}v7e}|I&gzaV==UTQykLkP;FUiof9Z$Vlg@= zP0q|>bWV|+r9bVomC-bG;2>vZ`E=+k9B};8B%h(eH9dHuKtveB#FxQHs2xT^RlNs2 zI*XFyijjZv5z988MV})5wn!Qne%6<4eI_R14=N6mBC#^~Tb$Chcvb3xq0sz@4J^PS zGt7!kP^QK7pY2%3Tv}~r;`LFl_C+E_o zoU0m5G@BHx3@3XBm%-+jCFyII>GX`MAp{wvN1BCvVN!`Y6hvj5B5V%wX;+Mww}n?I zHFK&4(Q^=khRija4+*L3#B;r;n;$_2ew4g1@;*l%PhN<;FnJ%t8*fsyUSYl!ha#qQ z=>sO@V@zWoV+@CU5Ge66?O)YzaJn9EosF)}<{O^HEO77zzf*7+kG)$73iIOeRXG{i4ftU0v!%I(q;$eYD z84Vrbt&|kcqu9{*;F0~=EHR^0$S94KuZg+VLY7N;-4t`RtFI(fxK=1%8*{BAaW8Nh zW2K4oFCD#f^vZC|RZS7f^W6+vPI}aSHyh?&Upjv2c&xZ3=2}T9*vtBuYr|cOiL7$K z0z^*1war&I$Gufkrv+~#K)5$AYM;;azI5i&nHL9N=^>V8ytHw~9xq)t*RoE`^WShy zS*Ojntm2lV!j_{SZaMZF;aHwOxMBmdQl()!VHCw(OR(G$tue#%&3ei&w6S}dwqqCkTz{r`G8MyK6%ILN^ zSk683!ZUNttHkClLh}}}d56%vW43wM^|IUHD^@UZ-_D!rzwyM)Ly)_@VoNP^>Y1B4 z?-jmVcx!EZ{T`ui&#Y(fTyfcy_xk!P_Ic2tcVF6lC3JcJr-cT04t@LO|UWE|4HL#|2h9yhCphJ#m%Vrq?&f zOLe6<1(ZzKGy;QWEf)f*-ysmL5f@O34zi@j+K!ka%sx5$MXjkMiwVcSs#J5t$}FYR zuU@I?|Ef}LPFPr9)Iw(@jc-ZnRXz3^th91zL{wm{g6&m~Nmnro=m~@ueWn9I3FQbG zFTagg6(DPa1EeH9;#5`vfzZPSvq7`%t0{j!XM}|1kC!1Uv!}V zU~CTseZ4*=73rf{tJpvfpCNcj*2No{{R!Xs0oXh^U!Rf0SUZ5?Kp5=ha8pQ#|3|9k z5P5$`-v3~c=SBudXG5}_xi~f&mNil$w7`1dB>)rXl(JQIT!bNcf>7e%VH!=fYLV7= zK}y5D9%-lWdkW_MN$Eva7cmBSb+z)Jr}aj-JOGjEWQgPn$Y06~is`J{q}XXH>pXzO-m7~RWq^228-zBq@~-e-F8l;s_O|F&vW(7MJ9G7nm{%+0 z)lTu#kKH&wlQwhcZCgBVUEH(&(UN47xT5{KT`X8ew%6;Yi)N0+3p(Q2TcX>3=&gD! z{QCHf@tKO5!CP5x4-24s9=u%?>pAj4snA7w08;bD zc>bz8o>h0j?{E9m#dh*z@l1cr?w7YBb<03+sN4@L+ln-bpMvX6SeQIXVp=1I>NLv+ zn9g)2OmRvHdXzjh&Y6-4uSy(vC3>~0AOL{Gq8Z`FWdIgUY!Ms~iymiOZ&3!+HfT#t zX#+M`f_Tj7iIqjHnmqw_9)q~Js&8!zmn(U((O~|OQxyO~CDqJtqqUWCe6p{1-ifI6 zS)2qcgSE-&v(SbYiNeg?u= z)5nzSdSGw`_V7TSk=$w+Tje2s1xiV9F=UY)NEZ)>WGR~$sWm15YiOJ)MKL|GBo2mx z&St_QPvUMS=-OS-kq%f8{|<@{{VD1O*v~ADXO=^VyR0T^dMW)1o%PNA@w{@e^A~rllz)wD)_suJLMfw2T9)8oihKKOwvt)%Crn2qMZzDiS|mt zUisSQnfiFurdfMCkjZHJXP?*%zAduI>bI)0w{2di?V_S<)A+()S=}p;Bu(S2+tWKCQ$D&Q~em0@nSv8r9Ltm8RWn@ zBj$!=JCtB_S!5H7J;=z;bhrTL9EjwhkPN$I%@v-ci1BD;WLJyX4MKK9Ji7^kRRyI{8%E!oe{I9n4U?Q$)+CfQ&Dd_{#J!tw zCUJUW&f=+cV%4fURjU}aZc{wFoz6(^{HS{|JBvD(VUu@KhE5$OGq!MZk_5Y1PiRsP zf7IK$F`Q0Rl;RjxZ_qQx{<8elfa{{;`p>r)+MtS78RG)Q(D3ah3tVUSEo*Ip!s z1H3W4^=S&IcmjmAb!e%2L#%%&STNu!1yyM&>9T1llmSmq8gK@IF5}anSKYO5|KnXe z?ej8_x#*);pU;4Y0#8JBH)DizRgzAnssg&tpNDFQlQ}ME}I>t(0 zBb_$Fx>ledU!rsH)3B_tdN|UO`#wEsC4HPd8sP3 z@6`vrnVHQy5HD*8IM{V^WRZ=}!Z1nBp8ZF*?F{VQ)xGC1Oa_mh>pR14oD0dAidEjK zc$&*HUiCZl#AdU)zrhS0Gf~b6oKle@TAYNfEZFd9+CvuR8`P#&Dvj-HG<&eG$-9~@ zVeEQ5f6*M5OP7!@^gOa)T6>qry$t|Er4_FRt_PxaQZg^EdTmddZyWrXQOX zdMT3kY;_;h<}K#9&YlfyJ1q&@)hxvY*qKsGb84}9M!8S-G zU`ON?lJWnd7?L_+(g`VOgdJvK*&PKs2G0{QKcPwd8z=;m*j*~RD+PCD++7WeAikQZ zGhcH@%`dqwxn}L9Fq@$~mxCU4t^8^^h{`25N~XgzJ@Kp!Bp#P)>7Z=#!nA2}WO~;- zr*HP(8i_sbkHfz9>f?|PKVdAGFDM7+B7LSaUa&rvzk%crH+>5I_jZVcc=Dndi&0EM z71-qdO*|w|Ze?OXx0T|V3!9ZE6`e@aLoFBSq!R38j*wBJp(^>4bW+o_%{P*!k|s>& z6I6aH??XNYaQS(9`Wy0SxEJV{I%Iy}aPJtOLqkZZQid>BQ9Y(xGvqsakz7aLJBhWZ zCi?{q?nV?!xFAzIozE?NWjR@{w_Hx2Z{P8Q!ksfcuhanFufE~_u&Ct@45eFeo>w$T zi*^~=`Q!E6tyc4bYe{JZv!VD;czB+!w?kdBiyHgcorIbYZKp~xQ@WBjf)lRpTk;Sv zdwXj^*vDC0+mAFU|4S*up~YaQQU^_G1#otgnj=w@QoBM@hb6xCvXXJ~5aM>)1!7$E znXB-3uNK}{je876%)N-=1OOpRvjG#mp=a<8cA#$VGWFoIwu)7Hv`Qz(C4mAo$8A!(}{t zHBFriSY^0P8DX-E7-{3sHls^`PAo))jSxduPd-X6%vlJU%PEyPsx&3}@>;rbH!j_yLRmCIm{nM+49Eo9gHu=`W6i{+m2|g!BEE(!@co|Z6_g>$rEX4 zym`@lOl&fJqkK?jGN#j*Z%3)}WW?BbX@Eb)02fVGgRbxzxZ2a)jp28KH+Q_Z=iNQG z&9_ejD8TN-rsMH7CxlwM^5H|*$)A{z^E0N4_(j+`F&l9Qv?+9Mm`&e=Di!~MGGFEO z^wd)d!zHPaIt-VjSO>MY6z%E56LNCpUy4=CcA1Qz@+J3 zV4C|ratbAD;A}$pmkg)tP`4MvyJ@ugQ3M7nuW+(%3RjM_zH{W}+FKpM#>4T|Jwg#A z8;?Yt^EvsGwwSL?C|G^dEUw!rtlJsebwXHoQYbhX&j~~wbMB&7xK~eKKRs=_J}kIb z+;Q)oaeTAqy`%3Qy%l=*gwUe##8Ic}2$f~~35OxV#Sbut#2Gsw=yqZ*A=n09xHhB@ zM7T-KMrWWQ{;q!jv}%|OIM-GU2?tZjs^KDFDp@tV96*V!N$e+m^-QZ~4jXlQ)UQ<^ zP=R@i^O~ zvZKOvN?{Ru5|c?4%HyDf#946XYGP|vmKfkUkOL?RELZYtzj_QTje%yAb&(pi^vVVIU7p`oO-?^>~u=5?_st zaDv1pAJhk{mDJiV>jC?M9)R=&zBI|;0MjRPe!+Pq?Q#uv6g+7fW%XIz^=SZoWn%+F z;}B}&&f)qBx)~zrI;|0y2{0Y#Ie=tl+;cZ18GcOjm?urR)Bp0uPwwnHbwIv74=7-| z4Ez`|kPeP8ojD4v4bo@`^qvB}IXM9LjgU!#^UTudkL0h~yyd>p2V<_XDbH~)NpFn! zNX;t2%G%%?&ta9wF=GQ`xHxen*p3r26Il%M?f0!XJafVJ`*xg-A*mN?A1{)#$e$(W zlsRL*Wn^fX^THd(5~^X$j3{m+OH@Y}r=%z9qvu?@D1*I2nvf`s!Qrtm z?(smoN6+JOL^XD^4Cu~5H9-$bmZdwHR^ewz zO4@Y%h?J8wD@bpFF$AS_riq9mIOrTc-HR(^P*vhiL&+mIASuKu4+*0+@HVQ84tkR9 z@R4oXx(*+f`Hz`0Dc4IM>A)RpuwjAX`Hv|1-;&3el2*Ark~6va>c{i{4<%&kL1vcw zf8aetl@WrVIT=;($oI*~&80rkk?W*EOdOUeMGm10W$++PlT#3NKqqn;u)1mowA`zr zdnSt@Y?)sz=B*I&R>bofqdVquOT^q3A-5&E9rud8?!MuUZl8rhd_@EO;;ymf^7{(* zzHDwq6TL5EwmZ9om0k4KG`D)axO$hcdKbO5vWv@Fg^E_P=0t_e7uCGn7~Mmb0nf_U zcHMM}>-Gxk_QossePXa>cxCH=bDm<+(x$0%~zqssE zr*%#CPYu>A?_Coz-t`zVyrQcLUnjbj3$EqU8)sc>@1~hE3W@Q*;ra&Urm&kfSWhnA ze#H)R8aXARw?*)_h~ABYccbX-5WF3@;3T(H%xx8NTgBXVA-7%3?G|#o#oQx8?h$m3 zY#|`-DwQumnai%7`ux=J%=(-DJMBHU*T*&;0e$CjV*xCDTzPu(iP!q3Yrl4O>ICRJ zS#6;2WRQJBPtGjeNY^anH{WqLzrB4fzg*036Y|@{{0-e=;X-|>|0Xi58Ci(_ZA<$LQ_>|AcSz1)K51%?%A z1vB*THbeh_tRNuOB@>`PmC!-)6!e0q93i5DsHZ?p?u9d+?ZoP6*Yw_#g&*l|0w%!& zAP9t$+QnRwh@n}x)HFJM(AiNw8z?PNenH+^4t)y?>;;u-Cs(ZvdSxnkm0xI+5SNI# zHxs|G#||V_54Bu?da6|0L^k{=B|He}9Hiq_ic|6}QL9Rmj#wD_4g>=lCZ>puih@|F zil*uzsY=-s4t$m4Az!8Y)w)tz`Xy;CH4OSvJ`%5|jS+`t@kkpp%)IKwsg*H8tBI18 z;s!CBQk`}q3-nC#cXur@p zv8ZUYGva(yG&-!J(Stfj@>v`@LN1i1gDi&#m8`%l+oH8XdD26Q^-Ga094Mn>#DVhi z9(M51DV3+o#IZM|y1^Z(A)qBCaWpQ1Fqtrth`R+6Pw9}PvvvQNPGU?x_q1F_`xYj* zkU;sWw=efs-Ukcfv0GjlA}6z_Gy2am_A}G` zR}C|=3&dnT9bp+V&h$P#{T)@qm>vuh`U86XM~e9W$RqrLzl$fyMFM_gv4a`5v(Q@t zyZSPvu_BG4obul(C;K@1F3C6m!;vE#Zk`Q6Oc3`BOBVJjp%CC`47rgFyAuktgxIfk zLoJ!uVz+!RZM#KD(oQJLED8n6Qd}bzuMvvZ#BlRxR&Fe>R?KS@@*3k=O;PI)-MMqF zY*1CBPyhPH$?QAM;`v?OxB0&;{6XH~TU);pdS&%gmJD@gw%!=Uy{R{IgbG-F-~3^I zM=b9!sfHZ>IIrlH)vtM{9{*Z#ytpx**A(3`@Am%s)3AV0PK+K%eGn%d7Pz{v!_0ew zSkfYtw8TqV#gg?x$@+K+OufsA3V}G?GdLaVqFr-2MU$(q?u&NK=T#+RR<%H{$QseC zs|sefU*C9Rqgd4@RJAFgzPXzE*8?{KvFg>)9kZ^A`BgAPzEfP+BdqI*uR0RlA-YH` zNp!6cTq|Z>EpvyCiidiIL%ly(d+K)XH!U}Vf9krk_Ec)RHV7SG8^1a}RsDM1jk@Wz z;56m0hvhMMzRc$m-L+67on5{`T>hA_{INUk$8IkZcL(n54*bwlKxht>P(&A5MVdS# zmaP)XR?Tb`%GUegu5#-Fb_%gC7;-U-Ik2=X6OB2GGqORC>7jzBB*Z{ZG)hfp z0hcRTNLiQhin_AV!W0*=fGtn0h9kobzmkWoP)DagPjrB(C)ytJ4fX*8^f6Rm016Pc z{fTsqwgj-G0&BS)`*uK|UiTnadP&T%?lXQwy+A=^?*15E8eY9 zxj4|tP*Qp_Wg}uGlgJv^1JsR1Afpeo8LY>u0Q(;l9_`92GKdHYhUp6?0I>b?QxYx1 z**#v~v+aP7_Cnv&1EF)c+jE>9o>kI;d=E=E`e6b8&5Qt|yxePrr!riCW`f~@%~E#S z%1AIqvKBH%jEsH_3r~#y5kRJ715Yx)W6dIdN80RLlj1v7NaZcgX_kX{*NV^RcPrz9GjoTpa?v56}LI|#iOBrB)`FtZ~c z{Z&AktSuv#N4lKy!QFZ)2swqICVs$wN~Kwtg&@f)6a5+NmTZadTLN5q1B*Xc=f2Rz ziJqXoCxZ&i=#gWu5qojsZq5xjj^m_>JEM~fEW3T8yGC%=#NBm(bVZPJz=a?w(hNuA zS?!Q!Sk@FU#lG90ix(e^Zoks@!d{$*AyfNtb}pvLbpO|f z-xz*#bT)T$JR7zvQ$;DJN~goq{WE)RS#Msrbuo7EiP*8@u>&V!|cd zR|wt}cbqFo9=l@G&7Pavaa1nvjAwU|NnCesG~?%=mK!`!5i-jCM0WnvQ+%wZ^VZ>8 zU9l=aygheZd&uktlHCsxz5lta&XtDmtaNnMnZC23va8bc-AW6d7aWi27|S5oDLgD? z8Vh$*>ZAB}nvGU6dx_yM~U*$;>3scld_mf@@D~{ z$X$_*(1rX#QLj8}ze+EnER}YFl#N;nL0;MyTP2OG!CQb0Rzp>_T@w9fwv zt@8?|W5v@L%7@8DifSfLNBc{@W$ceBF$s!Fj`O|H2~`R1lAWMaC>$J<%%^aHy5wOE z(8QTT{5{J1F%|z`$s_tcPx7BUJB7YQu|zG0jf2slbIicAdZ0|Qk3fc;+4pFm7`i{- zpxkAY+rsMQ{~Z-fG<2Cr&M4!wy(XyQGD$oG*6ny)IRXXPKzMY#bP?{796d^VS_eub zK?mq!#ofyw;g?-0W-k}Am&dak0CFmm>?A*uVdaGQHLUj(B`h^3Y$>}H`DuCUqzgle zeH2$PUsX44`PxQ$_OZMCI)$cAdRsBqx<+i>DYWjSx8}LBO0le2C~J(P(0pl-e2684H^u2e06m%T&?+d0uf)n%_gJkUb>-_o z1RR*qesb(!nRu2%WFHbL>LKX?-*6nWcGJJ@8yJmC2QulGsZ4m+8CFI#Y+ zQoj{S>elYl{mlJS$M`yR$*3ET4-CQv(njYmk}0_J130{>YF#1U!08bnz})un0xc^$ z;3x~-$2KnLhUL@#l}jEmrZjSsygM-DouM+l=z2vWkM;yUkCHP=6HN=+0_63dt)k3o z)#H_mHUR}=R?~bD#fAPON&Coq^<@B>S7haxv=NQ>E0W zOj6XTA|~=gC0`GbN|L#OjFEatHgW@XB%GYN2T3SDa!snt4z>+F)Jy*=hGcPlGY~nV zBB!`PUrGR8!<9;y9*ZJDY&YxCHbPZNbPdy^*9bJkynz0pTe@t1R8Q7tfoQNbp?Llu zur$ZLklY_TM`GD$7oN^#ig{iVty@D+7#8vwt!ze0&!#7)kfIT~{|;UB2sCb<#Jkz~ zoM8-#4k?{|WbA=4+u7DZP>QyW#NJ^?i)Dip1&el!M9hn{Py8dY0b|H67qjbx?7Db1 zP=p0|0`5~Nzfm6ZF(6$!-@IyO*B^Di-5qc1j5l{hZHlrtS^1lVe(BMtez2{v;1TPtkC-TOUFkV;FQCavPRjH&vaE`4gF~#XY z^CeQ!`7*^Vky2gSTJj}Qs^=*!Wxs0kn{J)>Rcx94Ob(UVCR8DOs~@eEPayI!&Dhvicc0s!)1Y3_WSUm<2iKiu#b%01_R*Q_%2)= zZ(k;#O$IPG#vS#(Fb)ub@pdqmim`LIXr)rDfYf0R!+9|em3T^PW3vJtPo5h_^2s@t;clw2$ejvzY$Bp+4 z#_SVD>BPy{Q@ybhy|~CSyU!T4lDc@Z0sOg4Tm(!e$zSid(J`|sR=IYDzghiW6%ZXf z^@K~EfcoVPDEIaBVd@UxLQ34kUbSfJUz}))rcNiyPtd}O1bdNe zyJNPBvAveMVr069-MaMz?m({IVf?AV=-8RK{xSLRM8qVvu^zxYPsZI8L3%wLPK`4o zcZyRES%fh`Ovc1iim+9wmdZ>?LM-a`1~nir6MT_vndzw#5vL1%L5?_GB(6^sl8E)$ z9IZsn#5v;46c190k+aehy9msQhZKjh)RL84_+_Ff5ZniP0IW>sz?PCPQ3qq}<0~eh ziB@liUG6blW;$?|tkwi&*|?Bo!d>OC(MUQR+hx1Q_WodRXaFMkAV&O8v`-UJl-cdy zrCsVT$@@OM1jPb(oqW(v0Kgw760;JY1&H$ys1yFOP-~&XDV{XIR zaI}Liw61x1Z?yAD{-u3$<<)p7Vprs3t9MpS*UhYr7k~vGJ$fkv;&oH$Lh*__#Ybk$ zV#_w6g)RU%8gDs*c>f)H!DsjKQ8QD2_`STxDwp8=dax3%8Nl&`v0Qdh2YNBpDIgY{ z&<%-M4GVh_E9Sdm)(HR-!b~<h(9ZA-em34$qRU2dS6+1%wLsgtbLCrwnkz*BzUFErB0N`uCYsJ zH+<+32GTj}TdJMB31`yz$f-wQh8Rwf))M7OS>NbOTxyiEcVtXtMlv+Pu!w57HnDfO zCy*kE6s>9(On4&hh=+5q-NG|t5md>yuc1^i%|yVQbv#5*9+ zF3)xRGW67G=!Zrw-Ns`395I>07o~KlxbdS<%MOh5?_o;eBBo`uvC!<2cc=UIO+&rI zr?}qD<4ud;BGi!j6E~M6v40ckZ&E?jl4lJ!dv4Xg8-UVc&r`zw-dJ~UY}2V{4GcGV z!zh_>^X#L12W&njzJ$JmZ$$a5(R|CoCi5%t{>!%ZT`Ggf2hSR&xasvbMq@RrXTmr4 z3Tt=8s&^+Ei;L3%E=Xjm2klOm?>0ae@Z+V3IL-Hl#(cZ>;K(cA=A!ZMtEsKE*?bk*WIQ7RUsQv@>0^`fLbW0jwlMwaDV1WDb!o{BvkCPfQec)|KplYOIB+ z9B@7O<-|D4@QbkQ`Yc9qm#nAx(Q{)V2Eimt|L8ebal1M{v~@dEOb%xmHyMg-jia#fpSB% z0|N!~WP{_(ydvML4c8l9Zi0@bBWS#|JG$*k?p%G-5AwH7<;+yijK^Ca`z>=Ue_M1X zJjtCu=gFmu=3(EzKxjTSTiW-bhr4T|ya0%fOtKIs*ja}27GgVi@uN;|L^|D{cuZ!h9Nup>^LMMH)`pbSumRCW< z{>moYR;SYcVRI}$UbYp%YjG)EMGYP{&}DQLbqVVf}@cgV(YNo0Q8%;8wxdYl}xHj*s*CJ^>BvYEd7-70+a-AaQemu^p9 zVJPS{{`^zVQUd0&`t7$uw}P>{y|bQusyO+-+_M_;j~heFF!ZmccbTofXNLDJkGIQ_ z_HAq4_B8u<9geOJ%XhOYyEa(9yTO9z1zdDGhBJ(~8w{W44bK-oU-W$OHN&L`-fT6I zhImYqREJ}`5yy7sCywsGh+im5g7mti2nF6V;pp`VJj2Yc2GfD$dM5E;lv2f6H8E6Z z0IJYiUo=~f61qf69b&=~Df3eMh46M=>L_jFDNZATyilykokfeIfHJ_6l7z*RMf4ST ztn?&=%2io1m?bDq5L9qI1VLiLZe;tsYI+A53k3B&FbpBdJBYMmm+YL|_J#v%Hc%k&ZHT*x;}Ttl}@Y7O7}(#o~G+w$rMmWnBij zDlX2%pMhm!o))4kqMW2SrEh^>0v@oFoei>fPT*~dFgoGD(4&+Es~HZQp1^@!2zL<< zoWyQ-aW>$tb_M^tR2Z=lrggc+Y`9byT8XcwQ+J9}S}~}j_m_O(19C$qFV$Tt()#R$ z!f0W%2(u|27gJ?~vEF2dG#Of}CNgmUS&=L*gXW^%YSv@;Ph=xUCd;8C4lv=3*e(@54b}6I1$bo^=cbes zIT5$+=-ZUCBH6k-K(-#~5NUN*HtuKl>XzZvEyKG+8Cf!pjCN&F%fnl>o>SLF4$kLE zU&m38uf2$pnyquxVGJIzLF z0p@QeGgV@v{k+I9ksEP?QJM=a*j&y@DY#tL$9WMq<_C7)iTwY+yKj$*<2v)~S3!5v zbT{ueG;f-x1n7;B5Dn-72qE;eEiBW>5|WTyjVy^lvc_>H;3#9scE)JNS)+|7Zf(3H zoMdNkHfIOhij60;XS>wxG$#1Oo6XtT*_l5uQX)Sx$^O1uT~*ypVau89oIP7Yw{P9L z_tved`}pqn_;Iy96=)Q9rK^UAz72aQ%cB!JQH3mku8*@LkP-?sMJEkhS7vVg@AUkTHnW`m} z7%=T<)$RnhcQ<}TX3M*XgQmZLo1U6H1p^ZYs7rC$99l`3d zeFm$Iwsh*dqxvuPc0xP4e?kjGd!#AXblU43GcMn*gYWM2?Rdbq;lWGdhK{dAuHj9C zryTI#4(xjDheBMfN(b!}NL!#rk`Eq(e2_nN(m}}fA;5XjIZVWpg9;G$wLe6JJerPZ z#yGeNOmu6ZjpGif-;HfX=Ao z34>sA$1IZv>S8<`Dq^mKdnW8#4b&1+@4~wYZ3O89MLZ>aUpC^%22IxH2G5nSl-%qY zLK@t0VdI3D0G;CAJ*2|THt=+x5Pwlr;cojfADlLpz1Wqj43N91*+Z-hQKvhzandke zKVg0^t3t?Z{NxUOswZs;+CRXP(F+ovjNeW-{lE4|zoOa|HT8@JuY0^`{QS3Sgp7ty z_#L?V2Y4D9;{PU3^J{vVun)9(vIrl@@?KUI+|7uO!))5v6om=)Et(^UpTIwi7|0bys)5uqrJR9dL>ZWH$QK8b`MR z2?w8VvOkc~;7@IU4HsSCJXIXX-0V-=Ji7J$jQp!j6OVbGN;YTnf#sZ1G@=1N$@~9Xz+MJcGJH!!U=e%5K&M*jGi^#%(11ee?!NxHF^WwPJKUj@DxJSg>paITi zsr87+vs98eOM$*3DoqE$F`KVwhvAkm;6SBaPQlEjV=KH~z>~Ht z?X#vV?Qn{+dcj>;$Q>)`-;yVkV}pG|8;9*!)x?+A8{*xu^_SFghm)hP4Z_*8vrx1~ zc@iy}GxY<3gRnB-?V5clI8ut|;A@LfC zy(VemdiZy!3Grga5KEXZN5kjLa9iE_r!@Pz|gGM5pj5hkl= z2pO@+z^rDR*`h8lsoaQcMJrDNFF1}6fxH=w#J5m>*1uWbV_lVzMvU3Wn!wnnJL$%j zQ#_zGfKh^V%ELP-DP9uS^|D^PnbP#ck?f|5Y0S)Vz=?tRszv^0C=J*aQ8o;Ac<2-N z4k{BJBQrhOcvTOF^DD#SvRl$b_*Xi4Uf#P}$fzB)iY7MMKqGzS)b&&T?A3QP$)@_Tb}#@}%_gM> zlL`g0ekl`@%3yz!m^ohmT=i)4d|Cms)N#P`dC*=U*b63H!J>Mhs6JSWpK%4-9~9aj47MK<+7E#{SoAPyHKZTbj&A*Ba#|qG9e4n`2#@$5=tJC}?8Z!- zkvn0XO#fm2)UK(b*K!5Nrs)JBui2mB8r}Y3M(!;BvWtS*^+I<2bCIu}jEy&E}Rd10qqr>Caj_;ha$<|TVLOOsF zL&i<@-n0OG$XOCY_NjdnwSG%U&{8E>s=V%>TIv>@cu5PD8gPqY=M=^*K6D@4;`!{n z38PT`U?BS;NK2 zCOvojh_@_|RyS&yO-g|tS9%6~zb54^YLZNaa3cqkVYaf(rewUZ|LXqnzULmgoze~S zcWnKa*aZnzddoq%tZW4)`1i?`u(*;B6!_-*;K_MC|X^uVj}3#EAIg>p@4-B0aX z#yyjLQ$621ekJ)z-*Xwa?OUd6r<;8C)=}>LxTI?wbmk`#uC1MDxwdH(zVQlSw-w)_ zg(tm|DsTVw!6}PS;<^$$ZoX|VBt@7O?JrZ(F|~VRZpPn;|E?_nu)CB)qvmK+vj4uW z5z=v6(Zu$N2Ji5X3a5_z=ZdKurU#!pn_D>1bnWQGS!mu*>U}x&Q00kV{h_rmGPAGs zpwIHaUG#~ENPJ*VA5R&5!e`9=WU*gMZ1YbRq0f=ftYu=cw=>P1IWcb+8xa0vt*NuX z@b|h{gb{3S+7!_LJEt*qg7^sGC}N|6bMcTiG0apI6G`-;t{ons&a0x>A=Ub2_2s2< z%iNWSMt0o4!Y}+@$nArORfWB+hJvDC#xO+@6$Z9yDOm%aw2T08%GM+}SR=he5k_56 zSXHy9IY287Tf)Yv1fePL*ANS7uFskJB<_Q_+7%%~heLMq@5cgndT6B-nKce7M zMw>pDSgrpnZcS_iRZhc-ymUzon`r#k`MKgd@t&dxH7C(k{L%GD3utMgwu#pv=5Ftl(wALfm;TO zkT6!~Y`eRf_I38|+u6ONYg|!}}3}Uxp1WSX81}nwM zH`sXFL=S<1{NVU>eXzMlXzuZ?-V-qHWf{mX2bqIWW{Qs&1vATq%yP0IgSu(_So{@t zFx4rfIs?XXmW#$LRc~`F9j^2ZS$`VA6S4k;gy`c^H@*O_M;{NOr>q zpS04~3QLoxUYlTKFBvvxIhnjtf zqHoE@PQG$KDW;49w4hK*Req)GYIv?NAyaKNBo!P?cVieoHWnIHK=^cTm3Rs) z6|;7S6lZXmb8%hmO`W}M-A%i@b~}^!r|9ZGQ!qkf_!kuGqTmTSC5f=8UUQM9QSem? z*qlJh#XQL*n7?qIrxM?x(@!Y)6oF)S;-E|U2>H60PW|DjxJ7&6lT|K@%2ZXMJ zz6TEZcJ}&uANE-elP|VD?T5DPdnwc&YWSxjxKLw`t5$!jmUC^0TsCE;l9#c7hEpYK z)0#9Sz>Zg4Cc}MqAqP=BQ3rB_1~41=@-(7kLk@#dS7tyjYdl-?dxgs6mO zFa`Ufa;0@rObtVjL7rDjIh5Z4q8+1-A!5n0V~B*8>`+`Wf~v+`L5K9qmW9<~T6m~h z_c|%Zkc^BlA_FxqQ@5;8A(d6g&w))!niRv5!b!^q!%`r0gfNso_!vzei5c1~nDQ$>E!t9WHGpT{AjTO@Xtwd2ruTx4usXbC$`ITxN z*r%jPG5Oj7DMk5}t^vwR?XJXuW`|1q3B6LK%!s)cUm;ZLAt{F5FQ-arctiSuL&}*H zlXi6wDxk&eTf_mx-8As9ltcL)=u_fS49e_oDX#nu98uy@ENoPm@4I%(=ht-k0Rs_t zN2PlxziMM_B#anBsR1Ro6jLoF_e3_HPfB@}Uq#W0<@l1LGHiB}ePCl8#f7EI(DX>R zNIhOum}00XRymC+0I@W?oW@j$+=&c%HwoiDS-!$lcHF5@bcs{G&iOQFdMCp~5#{LX zF{=%mfI)DEXLO+rfa{VKm*d!=2r)1m>j?pir86LGChhP@X32!Gy>U1eInE41n3D~) z1xbkDU^&C898QqMJ`U0W_f$HDb3$4UlFBmD9_jZCIl@FipXwctZDevf+=nTBY-Ccn zWB&`p0Kc7{!FuwfLz1wzw{YmdxXcDJ;*H1~)!t8)!c3$F<5GDWf1Dfl4; ze@nqM1q&3sK>;(Q_ESut;IAp5nL%^|n2lTWCn#W8EjQ=_liTdarcME3;lD)}7$>WM zVvI_Dkz%w6^UV}|LIKgd3ec!%fBh&`^))C*SjKs5piACVj`x8sZqDpkl3Fwwg zV0#9O8-(Hp0&*#cxKdgXEZr=WZl)_m3=gJLsP3e*3K=?`&T{8rVr6(G{(8JW3+z1r z$$(TBtyAeNCj?U}@$@q06H{NPzFO^1EI?D1IlW5^6J@>bfIsOVaAf5CkiauA zoRl12O7T_gt99OUH=nriguh}FH1E^$d}*a)L*D(ts?9>x=3tdesB%HPLX>{GU`!XW z!d^~$wSThhyMuxJjR9jLk^bOD293l|eDTbyh2H+}mIv}y1&phscLfs}-c>Lx%bQM{ zNt%wI*@p)DdJg*X9tap86kTqQ4+Tos1WPvwrJLYneR|bQ)0^v=(fhioqN&uWv(sm$ zANA#R`V)8hEIVg2bAy>Hh0K+cW&X?!W42M_mE!kp$;@5%Mxf03ERC}n`2fJP&ZcJJ zCk`^H@yD*EQ_8$Ovs$*R7+RVi4zg`D;NoCYaV z=6rI_b35Qi2?W)bPF*|YJr}HL6KdN0nOhe%I#U%47h0~=%~}$k z*)g_5G(H|b6U?g-h-F(ZG9%-PJm3GKDAi8VrIg}2$4v0_p`*)o;?+SaK9LOnr~)U_jDhc;z4C3Acwq*spu zifCd;NTJ-xD<-W|XQp?(es0DhY}x}ySH8vv(C+w$w0HF+Pb1k$?ozIl38NBYN^^&1 zxaGK~1h0G~HPdu%vSA*RQ+uCA7DFeRv6 z;!Lbe+m~sEjpkg&OB@L%(^f&!3b7>EWuB%y;_ipIm(-gOI<$=V(Be*@zyK@xr02hbG#DluGYzU-HVyq{*JYIrQ40X}FK*obmYT zcavCe5BiGEv{y?2RH42S{=N-p3F|BW4iOTZsnAra@1L|+_;LyQKn-$=&kEx`hTNE` z3g5TU&R;-+836@P=i(JxShzlA_TLSRZ0 z3<-@M7i*TdR9SmTlp*W=sQ3_zxPG3#-d0qAua^K|&@v&RESOLuB-8*^J1*hMk@4=U z1Hsht+o|RAN$KOM6S`}e!Hg;)qsn_&$XMY|TIqw--IgSRg8GaqJY`~turETi{^yKd z$`f_q&C&OaIwBp&N$(llMB?kSaWQ8nkMq4fJA1pjTNu|5Z;i3*$K){(-RY$@o?g$g zYdl?if?gTz_E?7x=dgQ=ZdTu@o=OwSoBXL;!2O!F;Z;uBFfy@U40QRa zVs>EbuMM}eWnQdg*%7RFSCXwbW9)s7#7K>Q571(BMl;vH37L!=B}p+RLvg&0&R8KdTThZCcz;zotvv3gNs zG4*HxH~`XU-E2+?V@1P*Pvf;lQUTc(rUGJ`E{1bMp=4{A9>_NBhXjGlVAXnFIduI{ zz*5hE*2pu^#5Mv&*2Tp4Xc=JJN+E6Kq!Z@9wcGq@+pmb=#$|%N%$po^HVV$hsUg49 zHRJZ#%L4Y@@2BN2BvIq;W-@;~8>Y5THTbf&`mNi1#%&+pjSU$~qVgQa6?3#t9-p&e zYR^>L^tzedGcCT-?x20QVBgLB@bmy^n7Bi4tX#cit?tdWrsi7iEp2IY757#Zhj@e( znC3_}qntw|oSlu)gAWybM9!bfv$e?zk3k!ONgjP*og|@ssCLY~1`@QBc%v1zQ6Jl|LPR}7PW>Pnj_Q=b2BPql4)QyN+iTzhf z;e_jz&g-3%&~97rPuT#?pVZ7?YN?P~>Ma*i>qp~cx_BaPa+S}R7cj1ywWThdvFZ8Q z`+XA4T}~=0zE$=%kt_Nb(K1wh9l0Qd5|2!YhEo*yTC_Gq9nw3-Rhbd5WRb42@1!=* z%2HJ}TicY9QcR+NAzPGZ+6du*lrii#a_*MraH4%xLd?^Mv=nRy2l^LAu5x2GN?aPj`X5E85tzs?>oy#lTVw%wnvI_^OZw)5^%PRnWv% z!JM$i3`G^DH>+0`7$`ZVcQ}x*q)9QX6smyBZ1vE=3#36|O=BFHmqATpZq}dLE|PJO*8tQJJD%10$Jk(gi1FA)$mQ9sLT$E>fT@EXt;W_)X3jkvl`m zykNtSNf12k!S&0jN62vi!x5M>9PfwQK_Xi)X^P!XDQpK9I z6Pz4cn(kpEUVz;Rbg!_{`Kiq{o;+DUW%k!KKWn&cbKSNboH6`2>$_Ipz5~Af2Yg!( zf?<-JP9XFUwldM`mCW0O(SrmahDmtq1R5MAH3ff!HOyDO-nV`SmXPi}GaGz)`vS)O zv$phm3DgcS6$*Z{h`myasD1cmrp>iN|K=vMtHJP=*?{oY3eL4IQa(ywgiUyVg~%U# zLD4Cbo*T=E`m`4(%jnoLL{p1AeV|dHgUHBN4bmit;lmL!9n-`6C9?GJ&$I3a>3?&} zjhN__hy-dP*OaYE2C>I7e&e4}xmbd^?LRyerKo2O5jUZ$DqAeE|AwxUtkFZ<*ZW(O z5_4XT^EPAe3uOJR!kVqV)m#0JZ9?KUaPA~N&P1$th^<%ks_PZ4BJA_Mil_LppHERR z#Vbeo=p%A>dqfuj>%*M<9X7M8e`+oBNgQes&EXKr8jv3p zyn?OZj%D+$itszCL&dV4iM-+|PYI+bF3idtqCGWjGg{Mz7WR6-uefuhd>ueIfIR0jpdb`R)l{XDq~9;qfNW%^l7?_GA*p9W(?DX3JShT!Jkloe)>iGC$l)4Jc%0T zY;3WDCZM$XU8QG;4ei^QXa@2KdISQ|42dS_RKe6?hGF6=xpbvuMVDPE+12mSEOY>k z!7K#WNLMhsO2~#~gpgh5Pp$t!|CB4(&>=K*%(#Vy-Tu`*K6o}Y&u3!ab2Ilw?o^E6 z+~m*PJZcknXKC5vyPi9@sIi*5wB#r@L&Th)7@l_dQcCKab_VS`1^Z66U+cnt zZ3Q~ToAG(B4BeZR>8^P0EyG&GZ^d)2RMV}*Sc+$uT9UY11*I)k?#EUR@d%%1@;>92 znAeu$lu0A*{vD=Z2BQT%>Hun;%5EbC)4y@nqy{GXH03IraA6ckARGmZFELc;&hOx_jk)7@jg;X{8jCoyb4b0DJ$u2IAml!CM|=XIoNr?UwiFFem$DtED>#VY35P1E#}Oy zp5^-=gEP1x(MhWX2`7#X9%oKkMcDbzw-2HLB@#5uQ_ib+Qa*{Oi|urQxWA$@#($*K zf1=ZWrhw>J=Dt{TyPKVP$P>X^R=jn*1$uPmelo;O zIiMXieJH(a1-O^N^_JECG_`%gFzKFl-d?@q*&$zQ`)C^i)|yq5k4_%&xsrb+7%{k+a3n6_0JX0i+j{~zXEi~Iuc$J$$( zcDKNIPuU;*2@RZZ-t~8AIC&9702a(i+yQsLr>}p|y)1Vibp01JbV3|!6>2*0ayc0&r*&x(POAsbLY=7pqHa0G5@3W(TB7yu^p()r&e-F zvIhT2<&jq^ePqs@6A>ayGjI&45t{<0q?@#C}`8H?!DLxD1fOD~jwY9OtyC zo8UC!Nk50CRPzxN?mp_ge#uUIUC7GC(Ahd%$wQ9b~ zi^8QY9BSCIT{v`|^cg)bp~?t>ILTQ8#xj|a%DS$al}NC$OK9v07+|#w1(_XR18Xdc^v^`=d3O3UOeRmD6C`?;&e1?^>1mpX7m5oJWoScQ}+&@ z+!aX9v*h`TzfO}DwwM})lK}VZ%vdqRv+O-{S_O+fI?G-9@cY<@SK}1W+*K?j7kh0& z@*2!tiCHfm_ZE6RlRd#zt-`8SUwx}Tw@pZFTZXflvUFaf`H0PH2B#6TiM)?g@=4d#v>=val4Gx=VP3)oy@XsIHvD>ADC+(nF+=1ME_7fQ(T%exOR57@aL$f znZj&h6+79d(IEac3Y015E}cftJCX{{X_!_otAaxpKckK}iYg*zoNyyo6;oLA)8;FO z$86sq00>e0GytEA(IS;pui>WohIwLSa-A=a>8p~V9eHgVf9!Ug1D3c4uOB3_kxQs^ z-7a>`CS}apvc;Z?S0Ep>+OeKmf<8+<6}?Bks6^!6BPZc2QMsrCJs?7p<%?ElRei=K zbub>dykaW1=h4=sT6sn=vDn*G;pBv_aF~NV?I;~uS)YyvJf1Q0 z8}V$>B%6-$brX9hw)!%v0>dI%N_ypC!iw>n^{S%`@>ZPCudo&nC?Rtk=l zlSP7KjX!hkkBX-a!Ogpb&2VbJSJ=GIzka_j4*#Kv9y{~JJFTd8Gzn59eFIY78 z{EzRh)Y#!CLQ4-F=6->c41Fc8>D|*U@Fx<}zFJ|o?ZcI~J44D?jkn-qTrzY1b$ z@gXF4vSR7x>E`KGzM^eG%XY!Cot569{S00u#3JnZWPcB0k(TMc={>$8n2@v!mUdR8 z14YcNhyf42w2hml^;2hjMJ)kK>rz00{NiAKt&m?kZ%?^$_&MW?#S_}UD0|uUO8fQp zAEZp4{eG^$Xfv2o1zWVEE|{(OEV&=r9G@-3(XBr945IJ85x=G-LGz|Pwbi75%bwMm zqIs(_t1Vu0%h+Vx%4vR_U}}xg|2VUtRjYqn%OU=@iEB-aeLKa}X4b!*ThPYo-{CmK z--$PE)#=|!Eoj@Qe`h0y_zY*-Zr9IPo7QfR)Bjx@hm!~%6G>YbEtVUi#ndP<%%SM^ z(Fam7BBBqZs_YezszOU4m1x1J1F4p%B%C5ug?>V+ye2ATkSa@gB2r~JPDE;|awf%4 z1F4us9Z1cLO2VmI`=a#%fSRDYC{s#m2qY4H!2cE+qWa@yn3cLvW4=m_vOMaGP)+1I z%ZCc3delt*6-|=+u2FlrocWe4 zaDU%jd--3^GdQjxnIZul=VSW)pZ5k-=o6xHpkYq{0jAHVL8SK>`Q(f~UlsL0IA!&k z87P3<2QTOb@p>ZKe3a2mU$V`RixGrDSl?kgztu}y;kJbM2J+qnUtrzReA2T;^g@;I z=Z;stmD_qj$LK6rAPgEoXGK^duq)g}4!*{0xQ3WQmpg5pPV1auTj#bOo(LYcujemN z>?sPqPQf!2e3OD}6ud;iUs7_L51iqbSCcaxQC&GnXHt%7!T*Rc!vN6kMj@SqfgD;9C^@dkX#o1(Otf zpMt-p;B^YJh*&yF0i)xXJv77dIYDWRtoxL1%1AgO@a9ZIJhX8hIot=&kw;GPCl3$7 z*?b@WBdYD|l$EhJeubDsof*j|?umZhGsiKu8Gn)Tw$cq+sK&U^!%{s6Nn54zqhG7t+d=l$`uv&I%!Cg>s2ZXIBWBE1;<=-t?m!Te5l7xZuzvq(QUR zn)J-kv7?s<;5ceL=IJMxDh?sdPNW9xWl%VJ;po+)&ksP@^}_b6+s8f6?*#8KEt^um zIq;!9l@VytAWS-slH>>7zp7MtHm`*fL!_!%d&{CF8Ezgk69N zVvU>!QEI+w942Vas0smi@k_1HQ6@0s8|By4VDI?=N*+>+tUJwoTSf_D$~bm2dE8H(YJ`cs|Xs zsNs+UuVUxbo&MzFU~+|!T;Z#3ocg1wlfKIKKyn9N?0B{VKDm=izDZ}naoJdj)& z_6BFOtG?L+5(}06?l%i2H1$(YGkV~y8cS=P=IuJuc8mV)hIQLx_3x}s-5Rf(v1k#W zi8pO)M^OV^lvE z-`m><`zq{hPn_uO#jE}SUJAny+{{$rc{jZz7SM$@JkG~YY3}M;wOsCEmo9>YO}em+QkMfw?n%aW9A;vE~ZL*LEk3M^MUF%rSF~u1MLQD{xSYk3ay;&SEr-x*K%zy*6)YNU+;;6^ zfq_d~wAi?e#R3j7YZBM2y;GinCthtpqQ%T*FQ#*7iq(jUk{w+BuU*;-u7WXa*_-Xd zo0SjDuHsH%5wAL2b6Rb05VkVQoH8lxf<{qtm($?SW56WeOOA}lygRJ%aZn3UR})iA0Exc$+Gk^`;$S4czg40w0qp#~SG+=oD0hveM~s6;*4 zPhQ@9HU!;TNY>l`CM}Wf2>an4fCuhF>*r$Jhk@2P2vbTT7vS6(FrMO2r0o$rW4PVD zgFunt$>?q_rspVu$-*h->0^WNnn`FRJR9Q)JNnN*a_TVe-Zlt{34iAF5FBP4Ju~D! zb#`#h0xf_c{?vI^6XDQOjXX4mWX)UCg4SHY znhW&!lh;p9xdPUvOKr1;xUY5mamQ0Tv3hYC#MUv6884G>Xj6iSD;kb5iuk+{E`i6a z?-|oSz8j~pewQTR<6rDYS<_92KM;{+wm-@Z(DBY4FTh_`VaT&r1(VJN~&8F{v-) zuNE#NnB!LxBh1;vYf=~V@T|L=#Z=beX~QCAgES7yK6UJDN;Qeu9)sg^+Ai`WKjHIO zesTbNidWj^pj4~p=;71-N@6}GT0BE;C8+>OBH~aHU5p(%QrpX;{$fg}-8f%L>8Xz& z^N@4nVbQw;`?mBQz=6=r0Tc@U9|k?g`g)I?I``-ad_)G_TrGWY(Y~#mYOA1t_O5&t z1=SSL=9FJS!Ac59cXcjrxjG5<86%djqpbB5{F(x$m^Xu|gx^KQziI+Ip+PlMnN1^1F;w7OquD6nd@t3S}>e4r`* zK(qD(&B_lnS&08!(;{eEKG2kWpmF|OQzK| limit + 1e-6: + continue + dist = abs(value - current) + if best is None or dist < best_dist: + best = value + best_dist = dist + if best is None: + delta = math.atan2(math.sin(target - current), math.cos(target - current)) + best = current + delta + if best > limit: + best -= two_pi + elif best < -limit: + best += two_pi + return best + + class GraspDemoDriver(Node): def __init__(self): super().__init__('grasp_demo_driver') @@ -101,6 +124,7 @@ def __init__(self): self._T_tcp_obj = np.eye(4) self._pending_pose = None self._ghost_pose = None + self._place_target = None self._displays = [] self._tries = [] self._selected = None @@ -136,14 +160,14 @@ def __init__(self): String, '/robot_description', self._on_robot_description, latched, callback_group=self._cb) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_broadcaster = TransformBroadcaster(self) self.create_timer(1.0 / 30.0, self._publish_tf, callback_group=self._cb) self.create_timer(1.0, self._publish_scene, callback_group=self._cb) self.apply_scene = self.create_client( ApplyPlanningScene, '/apply_planning_scene', callback_group=self._cb) - self.get_scene = self.create_client( - GetPlanningScene, '/get_planning_scene', callback_group=self._cb) self.plan_grasps = self.create_client( PlanGrasps, '/grasp_planning/plan_grasps', callback_group=self._cb) self.motion_plan = self.create_client( @@ -156,57 +180,28 @@ def __init__(self): self, ExecuteTrajectory, '/execute_trajectory', callback_group=self._cb) self.gripper = ActionClient( self, GripperCommand, '/hand_controller/gripper_cmd', callback_group=self._cb) - self.follow_joints = ActionClient( - self, FollowJointTrajectory, '/ur_manipulator_controller/follow_joint_trajectory', - callback_group=self._cb) self._publish_counters() self._publish_scene() def _declare_parameters(self): - self.declare_parameter('object_id', 'raw_stock') - self.declare_parameter('object_label', 'raw_stock_2x3x5') - self.declare_parameter('object_dims', [0.0762, 0.127, 0.0508]) - self.declare_parameter('object_base_quat_xyzw', [0.70710678, 0.0, 0.70710678, 0.0]) - self.declare_parameter('table_size', [1.2, 1.2, 0.04]) - self.declare_parameter('table_center', [0.3, 0.0, -0.021]) - self.declare_parameter('return_shift_center', [0.45, 0.0]) - self.declare_parameter('return_shift_bounds_xy', [0.1, 0.1]) - self.declare_parameter('return_shift_bounds_yaw_deg', 40.0) - self.declare_parameter('initial_object_xy_yaw_deg', [0.45, 0.1, 30.0]) - self.declare_parameter('min_place_distance', 0.06) - self.declare_parameter('random_seed', 7) - self.declare_parameter('group_name', 'ur_manipulator') - self.declare_parameter('end_effector_group', 'hand') - self.declare_parameter('tool_frame', 'hande_tcp') - self.declare_parameter('planning_timeout_sec', 10.0) - self.declare_parameter('gripper_motion_duration_sec', 0.75) - self.declare_parameter('retract_dist_m', 0.1) - self.declare_parameter('surfaces', [0, 1, 2, 3, 4, 5]) - self.declare_parameter('num_rotations', 4) - defaults = { - 'shoulder_pan_joint': -0.1597, - 'shoulder_lift_joint': -1.3542, - 'elbow_joint': -1.6648, - 'wrist_1_joint': -1.6933, - 'wrist_2_joint': 1.571, - 'wrist_3_joint': 1.411, - } - for name, value in defaults.items(): - self.declare_parameter(f'ready_joints.{name}', value) - self.declare_parameter('transit_velocity_scaling', 0.5) - self.declare_parameter('cartesian_velocity_scaling', 0.2) - self.declare_parameter('gripper_open', -0.001) - self.declare_parameter('gripper_closed', 0.025) - self.declare_parameter('gripper_nominal_stroke', 0.050) - self.declare_parameter('gripper_max_opening', 0.052) - self.declare_parameter('cycles', 0) - self.declare_parameter('pause_between_phases_sec', 0.4) - self.declare_parameter('motion_plan_service', '/plan_kinematic_path') - self.declare_parameter( - 'robot_description_web_base', - 'https://raw.githubusercontent.com/intrinsic-ai/intrinsic-moveit/{commit}/robot_hardware_description/') - self.declare_parameter('intrinsic_moveit_commit', '') + for name in ('object_id', 'object_label', 'group_name', 'end_effector_group', + 'tool_frame', 'motion_plan_service', 'robot_description_web_base', + 'intrinsic_moveit_commit'): + self.declare_parameter(name, Parameter.Type.STRING) + for name in ('return_shift_bounds_yaw_deg', 'min_place_distance', 'planning_timeout_sec', + 'gripper_motion_duration_sec', 'retract_dist_m', 'transit_velocity_scaling', + 'cartesian_velocity_scaling', 'gripper_open', 'gripper_closed', + 'gripper_nominal_stroke', 'gripper_max_opening', 'pause_between_phases_sec'): + self.declare_parameter(name, Parameter.Type.DOUBLE) + for name in ('object_dims', 'object_base_quat_xyzw', 'table_size', 'table_center', + 'return_shift_center', 'return_shift_bounds_xy', 'initial_object_xy_yaw_deg'): + self.declare_parameter(name, Parameter.Type.DOUBLE_ARRAY) + for name in ('random_seed', 'num_rotations', 'cycles'): + self.declare_parameter(name, Parameter.Type.INTEGER) + self.declare_parameter('surfaces', Parameter.Type.INTEGER_ARRAY) + for name in ARM_JOINTS: + self.declare_parameter(f'home_joints.{name}', Parameter.Type.DOUBLE) def _load_parameters(self): def doubles(name): @@ -235,8 +230,8 @@ def doubles(name): self.retract_dist = float(self.get_parameter('retract_dist_m').value) self.surfaces = [int(value) for value in self.get_parameter('surfaces').value] self.num_rotations = int(self.get_parameter('num_rotations').value) - self.ready_joints = { - name: float(self.get_parameter(f'ready_joints.{name}').value) for name in ARM_JOINTS + self.home_joints = { + name: float(self.get_parameter(f'home_joints.{name}').value) for name in ARM_JOINTS } self.transit_scaling = float(self.get_parameter('transit_velocity_scaling').value) self.cartesian_scaling = float(self.get_parameter('cartesian_velocity_scaling').value) @@ -279,19 +274,32 @@ def _robot_state(self): state.joint_state.position = [float(joints[name]) for name in state.joint_state.name] return state + def _world_tcp(self): + stamped = self.tf_buffer.lookup_transform('world', self.tool_frame, rclpy.time.Time()) + translation_msg = stamped.transform.translation + rotation_msg = stamped.transform.rotation + return matrix_from_xyz_quat( + (translation_msg.x, translation_msg.y, translation_msg.z), + (rotation_msg.x, rotation_msg.y, rotation_msg.z, rotation_msg.w), + ) + def _publish_tf(self): with self._lock: if not self._tf_enabled: return - if self._attached: - parent = self.tool_frame - transform = self._T_tcp_obj - else: - parent = 'world' - transform = self._T_world_obj + attached = self._attached + world_obj = self._T_world_obj + tcp_obj = self._T_tcp_obj + if attached: + try: + transform = self._world_tcp() @ tcp_obj + except Exception: + return + else: + transform = world_obj stamped = TransformStamped() stamped.header.stamp = self.get_clock().now().to_msg() - stamped.header.frame_id = parent + stamped.header.frame_id = 'world' stamped.child_frame_id = self.object_id stamped.transform = matrix_to_transform(transform) self.tf_broadcaster.sendTransform(stamped) @@ -383,7 +391,6 @@ def _result(future): def _wait_interfaces(self, timeout=180.0): services = [ (self.apply_scene, '/apply_planning_scene'), - (self.get_scene, '/get_planning_scene'), (self.plan_grasps, '/grasp_planning/plan_grasps'), (self.motion_plan, self.motion_plan_service), (self.cartesian, '/compute_cartesian_path'), @@ -393,7 +400,6 @@ def _wait_interfaces(self, timeout=180.0): actions = [ (self.execute, '/execute_trajectory'), (self.gripper, '/hand_controller/gripper_cmd'), - (self.follow_joints, '/ur_manipulator_controller/follow_joint_trajectory'), ] deadline = time.monotonic() + timeout next_log = 0.0 @@ -465,14 +471,6 @@ def _remove_world_object(self, object_id): scene.world.collision_objects.append(obj) self._apply(scene) - def _world_object_present(self): - request = GetPlanningScene.Request() - request.components.components = PlanningSceneComponents.WORLD_OBJECT_GEOMETRY - response = self._call(self.get_scene, request, 10.0) - if response is None: - return False - return any(obj.id == self.object_id for obj in response.scene.world.collision_objects) - def _attach(self): attached = AttachedCollisionObject() attached.link_name = self.tool_frame @@ -512,19 +510,41 @@ def _joint_state_from_map(self, joint_map): state.position = [float(joint_map[name]) for name in ARM_JOINTS] return state + def _wrap_positions(self, positions, current): + if any(name not in positions for name in ARM_JOINTS): + return None + return { + name: closest_equivalent(float(positions[name]), float(current.get(name, positions[name]))) + for name in ARM_JOINTS + } + + def _joint_score(self, wrapped, current): + return sum( + JOINT_WEIGHTS[name] * abs(wrapped[name] - float(current.get(name, wrapped[name]))) + for name in ARM_JOINTS + ) + + def _ik_acceptable(self, wrapped, current): + wrist_delta = abs(wrapped['wrist_2_joint'] - float(current['wrist_2_joint'])) + pan_delta = abs(wrapped['shoulder_pan_joint'] - self.home_joints['shoulder_pan_joint']) + return wrist_delta <= math.pi / 2.0 and pan_delta <= math.pi / 2.0 + def _plan_joint_goal(self, joint_state): last_code = None - for use_start in (True, False): + for pipeline_id, planner_id in ( + ('pilz_industrial_motion_planner', 'PTP'), + ('ompl', ''), + ): request = GetMotionPlan.Request() motion = request.motion_plan_request motion.group_name = self.group_name - motion.pipeline_id = 'ompl' + motion.pipeline_id = pipeline_id + motion.planner_id = planner_id motion.num_planning_attempts = 5 motion.allowed_planning_time = 5.0 motion.max_velocity_scaling_factor = self.transit_scaling motion.max_acceleration_scaling_factor = self.transit_scaling - if use_start and self._joint_snapshot(): - motion.start_state = self._robot_state() + motion.start_state = self._robot_state() constraints = Constraints() for name, position in zip(joint_state.name, joint_state.position): constraints.joint_constraints.append(JointConstraint( @@ -535,11 +555,16 @@ def _plan_joint_goal(self, joint_state): weight=1.0, )) motion.goal_constraints.append(constraints) - response = self._call(self.motion_plan, request, 30.0) + try: + response = self._call(self.motion_plan, request, 30.0) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f'{pipeline_id} {planner_id or "default"} plan call failed: {exc}') + continue code = response.motion_plan_response.error_code.val points = response.motion_plan_response.trajectory.joint_trajectory.points self.get_logger().info( - f'joint plan code={code} points={len(points)} explicit_start={use_start}') + f'joint plan pipeline={pipeline_id} planner={planner_id or "default"} ' + f'code={code} points={len(points)}') if code == MoveItErrorCodes.SUCCESS and points: return response.motion_plan_response.trajectory last_code = code @@ -583,93 +608,47 @@ def _execute_trajectory(self, trajectory, timeout=60.0): result = self._send_goal(self.execute, goal, timeout) code = result.error_code.val if result is not None else None after = self._joint_snapshot() - deltas = { - name: round(after.get(name, 0.0) - before.get(name, 0.0), 3) for name in ARM_JOINTS - } - self.get_logger().info(f'execute code={code} arm_delta={deltas}') + deltas = {} + for name in ARM_JOINTS: + deltas[name] = round(after.get(name, 0.0) - before.get(name, 0.0), 3) + max_abs = max(abs(value) for value in deltas.values()) + self.get_logger().info(f'execute code={code} arm_delta={deltas} max_abs={max_abs:.3f}') if code != MoveItErrorCodes.SUCCESS: raise RuntimeError(f'execute_trajectory failed ({code})') return result - def _move_joints(self, joint_map, allow_direct=True): - try: - trajectory = self._plan_joint_goal(self._joint_state_from_map(joint_map)) - self._execute_trajectory(trajectory) - return - except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'planned joint move failed: {exc}') - if not allow_direct: - raise - goal = FollowJointTrajectory.Goal() - goal.trajectory.joint_names = list(ARM_JOINTS) - point = JointTrajectoryPoint() - point.positions = [float(joint_map[name]) for name in ARM_JOINTS] - point.time_from_start = _duration(3.0) - goal.trajectory.points.append(point) - result = self._send_goal(self.follow_joints, goal, 20.0) - error_code = getattr(getattr(result, 'error_code', None), 'val', 0) - self.get_logger().info(f'direct joint trajectory error_code={error_code}') - if error_code not in (0, MoveItErrorCodes.SUCCESS): - raise RuntimeError(f'follow_joint_trajectory failed ({error_code})') + def _move_joints(self, joint_map): + self._execute_trajectory(self._plan_joint_goal(self._joint_state_from_map(joint_map))) def _cartesian_to(self, transform, avoid): pose = matrix_to_pose(transform) - last = None - for use_start in (True, False): - request = GetCartesianPath.Request() - request.header.frame_id = 'world' - request.header.stamp = self.get_clock().now().to_msg() - request.group_name = self.group_name - request.link_name = self.tool_frame - request.waypoints.append(pose) - request.max_step = 0.005 - request.avoid_collisions = bool(avoid) - request.max_velocity_scaling_factor = self.cartesian_scaling - request.max_acceleration_scaling_factor = self.cartesian_scaling - if use_start and self._joint_snapshot(): - request.start_state = self._robot_state() - response = self._call(self.cartesian, request, 30.0) - points = len(response.solution.joint_trajectory.points) - self.get_logger().info( - f'cartesian avoid={avoid} fraction={response.fraction:.3f} ' - f'code={response.error_code.val} points={points} explicit_start={use_start}') - last = response - if response.fraction >= 0.95 and points > 0: - return response - return last - - def _execute_cartesian(self, transform, avoid, fallback_joints=None): + request = GetCartesianPath.Request() + request.header.frame_id = 'world' + request.header.stamp = self.get_clock().now().to_msg() + request.group_name = self.group_name + request.link_name = self.tool_frame + request.waypoints.append(pose) + request.max_step = 0.005 + request.avoid_collisions = bool(avoid) + request.max_velocity_scaling_factor = self.cartesian_scaling + request.max_acceleration_scaling_factor = self.cartesian_scaling + request.start_state = self._robot_state() + response = self._call(self.cartesian, request, 30.0) + points = len(response.solution.joint_trajectory.points) + self.get_logger().info( + f'cartesian avoid={avoid} fraction={response.fraction:.3f} ' + f'code={response.error_code.val} points={points}') + return response + + def _execute_cartesian(self, transform, avoid): response = self._cartesian_to(transform, avoid) - if response.fraction < 0.95 or not response.solution.joint_trajectory.points: - if avoid: - self.get_logger().warn('cartesian fraction low, retrying with collisions ignored') - response = self._cartesian_to(transform, False) + if (response.fraction < 0.95 or not response.solution.joint_trajectory.points) and avoid: + self.get_logger().warn('cartesian fraction low, retrying with collisions ignored') + response = self._cartesian_to(transform, False) if response.fraction >= 0.95 and response.solution.joint_trajectory.points: self._execute_trajectory(response.solution) return - if fallback_joints is None: - raise RuntimeError(f'cartesian path fraction {response.fraction:.3f}') - self.get_logger().warn('cartesian path incomplete, interpolating joint goal') - self._execute_trajectory(self._interpolate(fallback_joints)) - - def _interpolate(self, target_state, duration=2.0, samples=20): - current = self._joint_snapshot() - names = list(target_state.name) - target = [float(value) for value in target_state.position] - start = [float(current[name]) for name in names] - trajectory = JointTrajectory() - trajectory.joint_names = names - for index in range(samples): - alpha = float(index + 1) / float(samples) - point = JointTrajectoryPoint() - point.positions = [ - start[i] + alpha * (target[i] - start[i]) for i in range(len(names)) - ] - point.time_from_start = _duration(duration * alpha) - trajectory.points.append(point) - robot_trajectory = RobotTrajectory() - robot_trajectory.joint_trajectory = trajectory - return robot_trajectory + raise RuntimeError(f'cartesian path fraction {response.fraction:.3f}') def _fk_pose(self, joint_map): request = GetPositionFK.Request() @@ -682,25 +661,61 @@ def _fk_pose(self, joint_map): raise RuntimeError(f'compute_fk failed ({response.error_code.val})') return response.pose_stamped[0].pose - def _validate_ready(self): - pose = self._fk_pose(self.ready_joints) + def _log_home_fk(self): + pose = self._fk_pose(self.home_joints) tool_z = quat_to_mat(( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))[:, 2] self.get_logger().info( - f'ready hande_tcp xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f}) ' + f'home hande_tcp xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f}) ' f'tool_z={float(tool_z[2]):.3f}') - if float(tool_z[2]) > -0.3 or pose.position.z < 0.05: - self.get_logger().warn('ready pose failed the TCP check, using SRDF home') - self.ready_joints = dict(HOME_JOINTS) - pose = self._fk_pose(self.ready_joints) - self.get_logger().info( - f'home hande_tcp xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f})') def _publish_candidates(self, displays): self.candidate_pub.publish(build_candidate_markers( self.get_clock().now().to_msg(), self.object_id, displays)) + def _annotate_variant(self, item, current): + positions = { + name: float(pos) + for name, pos in zip(item['pre_ik'].name, item['pre_ik'].position) + } + wrapped = self._wrap_positions(positions, current) + item['pre_joints'] = wrapped + if wrapped is None: + item['score'] = float('inf') + item['accepted'] = False + return + item['score'] = self._joint_score(wrapped, current) + item['accepted'] = item['feasible'] and self._ik_acceptable(wrapped, current) + + def _ik_fallback(self, groups, current): + found = {} + seeds = [] + for key, items in groups.items(): + feasible = [item for item in items if item['feasible']] + if feasible: + seeds.append(max(feasible, key=lambda item: (item['quality'], -item['approach_z']))) + seeds.sort(key=lambda item: (-item['quality'], item['approach_z'])) + for seed in seeds: + world_pre = self._T_world_obj @ pose_to_matrix(seed['pre_pose'].pose) + try: + joints = self._ik_pose(world_pre, reject_far=True) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f'pregrasp IK fallback failed: {exc}') + continue + wrapped = {name: float(pos) for name, pos in zip(joints.name, joints.position)} + chosen = dict(seed) + chosen['pre_joints'] = wrapped + chosen['score'] = self._joint_score(wrapped, current) + chosen['accepted'] = True + found[seed['key']] = chosen + self.get_logger().info( + f'pregrasp IK fallback score={chosen["score"]:.3f} ' + f'pan={wrapped["shoulder_pan_joint"]:.3f}') + break + return found + def _select_candidates(self, response): + current = self._joint_snapshot() rotation_world = self._T_world_obj[:3, :3] groups = {} for index, grasp in enumerate(response.grasps): @@ -715,7 +730,6 @@ def _select_candidates(self, response): 'grasp': grasp, 'pre_pose': response.pre_grasp_poses[index], 'pre_ik': response.pregrasp_ik_solutions[index], - 'grasp_ik': response.grasp_ik_solutions[index], 'width': width, 'feasible': width <= self.gripper_max_opening + 1e-6, 'approach_z': approach_z, @@ -723,51 +737,61 @@ def _select_candidates(self, response): 'key': key, 'pose': pose, } + self._annotate_variant(item, current) groups.setdefault(key, []).append(item) - def best_feasible(items): - feasible = [item for item in items if item['feasible']] - if not feasible: - return None - return max(feasible, key=lambda item: (item['quality'], -item['approach_z'])) - - ordered_keys = sorted(groups, key=lambda key: ( - best_feasible(groups[key]) is None, - -(best_feasible(groups[key])['quality'] if best_feasible(groups[key]) else max( - item['quality'] for item in groups[key])), - min(item['approach_z'] for item in groups[key]), - )) + best = {} + for key, items in groups.items(): + accepted = [item for item in items if item['accepted']] + if accepted: + best[key] = min(accepted, key=lambda item: (item['score'], -item['quality'])) + if not best: + self.get_logger().warn('no close IK variant, seeding /compute_ik from the current state') + best = self._ik_fallback(groups, current) + + def sort_key(key): + if key in best: + return (0, best[key]['score'], -best[key]['quality']) + items = groups[key] + feasible = any(item['feasible'] for item in items) + quality = max(item['quality'] for item in items) + approach = min(item['approach_z'] for item in items) + return (1 if feasible else 2, -quality, approach) + + ordered_keys = sorted(groups, key=sort_key) displays = [] feasible_rank = 0 - for index, key in enumerate(ordered_keys): + for key in ordered_keys: items = groups[key] - chosen = best_feasible(items) or max(items, key=lambda item: item['quality']) + chosen = best.get(key) or max(items, key=lambda item: item['quality']) pre = chosen['pre_pose'].pose.position grasp_position = chosen['pose'].position + feasible = key in best or chosen['feasible'] displays.append({ + 'key': key, 'pose': chosen['pose'], 'pre_position': (pre.x, pre.y, pre.z), 'grasp_position': (grasp_position.x, grasp_position.y, grasp_position.z), 'width': chosen['width'], 'quality': chosen['quality'], 'n_ik': len(items), - 'feasible': chosen['feasible'], + 'feasible': feasible and chosen['feasible'], 'selected': False, 'rank': feasible_rank if chosen['feasible'] else 0, }) if chosen['feasible']: feasible_rank += 1 - tries = [] - for key in ordered_keys: - chosen = best_feasible(groups[key]) - if chosen is not None: - tries.append(chosen) - tries = tries[:3] + tries = [best[key] for key in ordered_keys if key in best][:3] if displays and tries: selected_key = tries[0]['key'] - for display, key in zip(displays, ordered_keys): - display['selected'] = key == selected_key + for display in displays: + display['selected'] = display['key'] == selected_key + accepted_n = sum(1 for items in groups.values() for item in items if item['accepted']) + self.get_logger().info( + f'IK variants accepted={accepted_n} distinct={len(groups)} ' + f'selected_score={tries[0]["score"]:.3f}' if tries else + f'IK variants accepted={accepted_n} distinct={len(groups)} selected_score=none') return displays, tries def _publish_selection(self, candidate): @@ -779,12 +803,17 @@ def _publish_selection(self, candidate): self.selected_pose_pub.publish(stamped) self.selected_msg_pub.publish(grasp) + def _mark_demo_ready(self): + with open(READY_FILE, 'w', encoding='utf-8') as handle: + handle.write('ready\n') + def _phase_init(self): self._wait_interfaces() self._add_world_box('table', self.table_size, translation(*self.table_center)) - self._validate_ready() - self._move_joints(self.ready_joints, allow_direct=True) + self._log_home_fk() + self._move_joints(self.home_joints) self._gripper(self.gripper_closed) + self._mark_demo_ready() return 'SPAWN_WORKPIECE' def _phase_spawn(self): @@ -845,15 +874,16 @@ def _phase_plan(self): self.pregrasp_poses_pub.publish(pre_poses) self._publish_candidates(displays) self.get_logger().info( - f'{len(displays)} distinct poses, {len(tries)} feasible fallbacks, ' - f'best q={tries[0]["quality"]:.3f} width={tries[0]["width"]*1000:.1f}mm ' - f'approach_z={tries[0]["approach_z"]:+.2f}') + f'{len(displays)} distinct poses, {len(tries)} close fallbacks, ' + f'best score={tries[0]["score"]:.3f} q={tries[0]["quality"]:.3f} ' + f'width={tries[0]["width"]*1000:.1f}mm approach_z={tries[0]["approach_z"]:+.2f}') return 'SELECT_GRASP' def _phase_select(self): self._publish_selection(self._selected) self.get_logger().info( - f'selected grasp id={self._selected["grasp"].id} q={self._selected["quality"]:.3f}') + f'selected grasp id={self._selected["grasp"].id} ' + f'score={self._selected["score"]:.3f} q={self._selected["quality"]:.3f}') return 'OPEN_GRIPPER' def _open_position(self, grasp): @@ -871,18 +901,12 @@ def _phase_pregrasp(self): for index, candidate in enumerate(self._tries): self._selected = candidate for display in self._displays: - display['selected'] = False - if self._displays: - self._displays[0]['selected'] = index == 0 - for display in self._displays: - if abs(display['quality'] - candidate['quality']) < 1e-9 and abs( - display['width'] - candidate['width']) < 1e-9: - display['selected'] = True - break + display['selected'] = display['key'] == candidate['key'] self._publish_candidates(self._displays) self._publish_selection(candidate) try: - self._execute_trajectory(self._plan_joint_goal(candidate['pre_ik']), timeout=60.0) + goal = self._joint_state_from_map(candidate['pre_joints']) + self._execute_trajectory(self._plan_joint_goal(goal), timeout=60.0) return 'APPROACH' except Exception as exc: # noqa: BLE001 last_error = exc @@ -892,7 +916,7 @@ def _phase_pregrasp(self): def _phase_approach(self): grasp_pose = pose_to_matrix(self._selected['pose']) world_grasp = self._T_world_obj @ grasp_pose - self._execute_cartesian(world_grasp, avoid=False, fallback_joints=self._selected['grasp_ik']) + self._execute_cartesian(world_grasp, avoid=False) return 'GRASP' def _phase_grasp(self): @@ -928,7 +952,7 @@ def _phase_place(self): place_tcp = target @ grasp_in_object pre_place = place_tcp @ translation(0.0, 0.0, -self.retract_dist) try: - joints = self._ik_pose(pre_place) + joints = self._ik_pose(pre_place, reject_far=True) with self._lock: self._ghost_pose = matrix_to_pose(target) self._place_target = target @@ -940,7 +964,8 @@ def _phase_place(self): self.get_logger().warn(f'place sample {attempt} failed: {exc}') raise RuntimeError(f'place planning failed: {last_error}') - def _ik_pose(self, transform): + def _ik_pose(self, transform, reject_far=False): + current = self._joint_snapshot() request = GetPositionIK.Request() request.ik_request.group_name = self.group_name request.ik_request.ik_link_name = self.tool_frame @@ -953,11 +978,18 @@ def _ik_pose(self, transform): response = self._call(self.ik, request, 10.0) if response.error_code.val != MoveItErrorCodes.SUCCESS: raise RuntimeError(f'compute_ik failed ({response.error_code.val})') - positions = dict(zip(response.solution.joint_state.name, response.solution.joint_state.position)) - missing = [name for name in ARM_JOINTS if name not in positions] - if missing: + positions = dict(zip( + response.solution.joint_state.name, response.solution.joint_state.position)) + wrapped = self._wrap_positions(positions, current) + if wrapped is None: + missing = [name for name in ARM_JOINTS if name not in positions] raise RuntimeError(f'IK solution missing {missing}') - return self._joint_state_from_map({name: positions[name] for name in ARM_JOINTS}) + if reject_far and not self._ik_acceptable(wrapped, current): + raise RuntimeError( + 'IK solution too far from the work-facing pose ' + f'pan={wrapped["shoulder_pan_joint"]:.3f} ' + f'wrist_2={wrapped["wrist_2_joint"]:.3f}') + return self._joint_state_from_map(wrapped) def _phase_descend(self): grasp_in_object = pose_to_matrix(self._selected['pose']) @@ -969,8 +1001,6 @@ def _phase_release(self): self._gripper(self.gripper_open) self._detach() self._add_world_box(self.object_id, self.object_dims, self._place_target) - if not self._world_object_present(): - self._add_world_box(self.object_id, self.object_dims, self._place_target) with self._lock: self._T_world_obj = self._place_target self._attached = False @@ -988,7 +1018,7 @@ def _phase_retreat_up(self): def _phase_park(self): self._gripper(self.gripper_closed) - self._move_joints(self.ready_joints, allow_direct=True) + self._move_joints(self.home_joints) self.cycle += 1 self._publish_counters() self.get_logger().info(f'cycle {self.cycle} complete') @@ -1017,14 +1047,12 @@ def _recover(self, reason): except Exception as exc: # noqa: BLE001 self.get_logger().error(f'recover gripper open failed: {exc}') try: - self._move_joints(self.ready_joints, allow_direct=True) + self._move_joints(self.home_joints) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f'recover return to ready failed: {exc}') + self.get_logger().error(f'recover return to home failed: {exc}') try: - current = self._joint_snapshot() current_xy = (float(self._T_world_obj[0, 3]), float(self._T_world_obj[1, 3])) self._pending_pose = self._sample_pose(current_xy, enforce_distance=False) - del current except Exception as exc: # noqa: BLE001 self.get_logger().error(f'recover resample failed: {exc}') x, y, yaw_deg = self.initial_xy_yaw @@ -1059,6 +1087,7 @@ def run(self): self._set_status(phase) started = time.monotonic() try: + done = phase phase = handlers[phase]() except Exception as exc: # noqa: BLE001 self.get_logger().error(f'{phase} failed: {exc}\n{traceback.format_exc()}') @@ -1071,7 +1100,8 @@ def run(self): time.sleep(1.0) phase = 'SPAWN_WORKPIECE' else: - self.get_logger().info(f'{phase} next after {time.monotonic() - started:.1f}s') + self.get_logger().info( + f'{done} took {time.monotonic() - started:.1f}s -> {phase}') if phase != 'DONE': time.sleep(self.pause_sec) diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp index 9aa6adb..c8781b7 100644 --- a/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/moveit_planning_service_standalone/src/moveit_planning_node_standalone.cpp @@ -24,9 +24,7 @@ #include #include #include -#include #include -#include #include "moveit_planning_service/grasp_planning_pipeline.hpp" #include "rclcpp/experimental/executors/events_executor/events_executor.hpp" diff --git a/integrations/ros2/intrinsic_moveit/scripts/__pycache__/check_motion.cpython-312.pyc b/integrations/ros2/intrinsic_moveit/scripts/__pycache__/check_motion.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c6c22e21487a9c0295c13fdc8a5162f1a73cf094 GIT binary patch literal 5758 zcmb7IZBSI#89w*!-S1D>#pPp_RX|yy5JCJZiU_NqfCMm2HZkkE_ks)i)q5`@+a*Ec z46LOiPLql;nPB5o9Bt!I+i^NJ?X;QZ&n{rZE1BA+KgvIf{!sg)eb3#!yNjAkduR5X z_nh;*?>*=JI`7@D^m+|~=XZbqZQm|ELf_(n^r%wAquT^R7Z8nT!jD|Zo^TQ3m~@e1 zFLTKlvQOfWBtYM%%^IXg8ZpF6v9N>ZpYK2#pT{A@sIJ34@ssY!-T}E+D<~Id$Tsb05`EvtimzhAlh?f6?a9N;N zK%Yk|U8`u7%Sx-yAXh$9Kx_KT5n|P91n9Ya4kk(6QW^AWPnsC|PvJDc0=%nmTG)OoMikIZe z6hba{JT%~^8P*;41l@fYB$(54eqRrtn$$A>W1+#+KtIU(INrT^<*<6?uqHWlC{6An#~xXmkoNZgnw2{^dT>KM2^rf86|ts&&Y8l1vA6sSzRwWs~^+#5VZV^YFIX=2TB1H#pk3YDfwK9 zepw9htJ16a5|!z1sIFsO!!s<85`9HUI=kg<(uh_^WwhpcO6vsr4*Z%lUK^#N@(i9| zE3Ndclc-ED=`)J3UWOG}S}o0>BuYZFuK!W0q96Q*ZOoFoUXqpRqXezHq0hu{7W(`4 zkpF+}ff#S}p~UZUut zPts8?%C?DIt6$(+16;{9%vJmxmtC~LcIjMwJ0&X9zaVifNEG>KEOA}7L_U2(fu%H~ z8ZYD)us6KqtcI_bWR7Vdr<9yg#q>=`@;*reEHR!R(}4Yp!2a4{O;pPovUE@y)#AHQ zLZ2qG>{<35Hd&w6{07)>d0LBjG~?56q@=arTN7U0E4~fY>Br zvJ*I-wJ}*#yzBbXH)Do7qnT*hsu{hGLk1Z_*NAq9T#!Y4VSx&I0*s&vhd7^jOob;L zW`eXJ@9~E`yn_;C0j`%_4f0uhDF{l=6A1emj>Frw+wCLIoK=G%wx0`oyi64oDoiiy z!IHQtHpErq87OKvpBMCag7NWHoR{^5d9Es3xs~A|K^;m}I$XpW@c4q2b^ego$WoJEf?~g6N-v74bIG{G>i& z6@lqW8s0B%!XA-T*7QWQSgL*ZF8k)nS{9dqsgo?$M{-5RWPx3s;z`3_t=!Jy@2eH4 zniNVS2@U&R@Hc$bKEgH3hr~fdz=vQI9f1uG6EO((6X@r}1%g218XU5}k_{3pF72*i z8bmKkp>zg*O1tW+TN z-#UDUzI`N7cl4?tF- zv2AyOTDSB>G`p?D8(U&;De>5VQ;IG#R?LP-JK2X zT`kV`Zg=aU#)ekGoD#te982aUI1r1IIEynYs2IFgPzOTbPNAUBE0CdnLCuAIL3b$V zWdym8X966gNY6=Vd_h6i*3$0AJL&3ZXb0Os>H`z8p#jLKCcK8fxtdb@6f|8!90+ip zgu_q)4+I?<_6f#L=WB;sIsw+v4BuK^g3J>f5_B}f@oZ=)d0G{Be1NAzgF%NuP~q4V zb2o)Gk<8Vl_zXA+2ui<)hg?eoj`7fbUl6t}2A-fe;qebJoFEs&i`^=sQ#NMTVF5TQ zIsCl?i9YolweQ)JOhvF`UyywpRcE-S1+D3x)|SxPCf}LWZX9V^D6NcFAB=BqO_a8N zZD^Zp8sjHAV<%?}Z6o{VjX5LDk13ga>thWmU;kKzj0N)s(}Jb!o~1Hjsa!CZ+%wlC z%r%ddluU)$eLD@cbO&8yC-07U7n&&B1YVP%^cW%CUM+tKaM)SFMWADxy zi>JyH#tPV<#(1tgRz67uU=dIHsFt3brh$^yiE*<0Lg*a#vOPScbHw7htmm zTb2|^Zya&X=NEsfzo@@tTt<|9O-wgNjrH8u<}4Jhp6t2w$_?4{zU$gVVa=FxqV(*+ zhlQ(?#F84ok2R>+@wIOM1U>D%S$gf@?Txb=_nqyI>-LX0p#k-cZMswTbwTs3(#g^* z@+oGz>2m+g&9nB}L}BerLGv8df-w~n$LFZx1*K-J_Wj75rF2GFx?sqkH{^e8o;-Z1 za8eg9-SbD=9mAJ&T-WtTMi>thONdY(CLStvasB#)a>I1(jPh0Bi5F~{HPpscwexso zZ9=(j$~&WUh|+Mr{Q|~3# zZo4Im+v{!}jO&^){(y)-kj7uJT(w@d&M3>pm#pMD07}3-Eh0|~@sD+z?-@LkdAH58 zbw}erbkEfty}RS+*@3vhGqN9=`$_{oL{ZgE<@8XzV8^UsXI!-tR5Y(yd!=NG`K)5f z1}Cq~pU+<%U$ZTdzx|duZrue_yDjsUf_NeLSJh2f+`I**YOV8D+oU^T-8Aiuud0Tb znipqs7jpAH2+iiMkLPSy(#qA2$1;VyV#y%7WL|>Go1?5a3ffP#-=!>1mpK9%+li;k zJCVMbcq$Y$KV7yX<1vE6Vf_0VO=~^%r>dMbiuyA_;Wo$Grq=vrLqVH?`U++Yl%R6E zaX7l&5xJPL;p(OE|4ytGBshxMcIY5`aqJEtSX@I1R2XL*1r%ot%L_`{$BLz((sK;b z1FV2ZEWzy-lu*gJncxXQ!34PhmSKN@`PE`BuKW=Q#mO}*>l!)0g;+OKj$9=!A8|$% zbR17-rf_~`cjKl5H!Zkn#SKm<_}zFjZa3S8hid0ZFBBz^oCO(k(#syelM0Z>uq=x& zQL%tYUVf^&K!_gjGrQR!Ea2jtn}KFYMi9ieNc#Y(A0W*GqsT&}pQ+H~KLKbiBBz?3 0.5] + moved = [name for name in ARM_JOINTS if spans[name] > MIN_ARM_SPAN] print('joint spans', {name: round(spans[name], 4) for name in spans}) print('statuses', statuses) - if len(moved) < 3: - raise SystemExit(f'FAIL only {len(moved)} arm joints moved more than 0.5 rad') + if len(moved) < MIN_MOVED_JOINTS: + raise SystemExit( + f'FAIL only {len(moved)} arm joints moved more than {MIN_ARM_SPAN} rad') if spans['hande_left_finger_joint'] <= 0.005: raise SystemExit('FAIL gripper did not move') + if spans['shoulder_pan_joint'] >= 1.5: + raise SystemExit( + f'FAIL shoulder_pan span {spans["shoulder_pan_joint"]:.3f} rad >= 1.5') + if spans['wrist_2_joint'] >= 0.8: + raise SystemExit( + f'FAIL wrist_2 span {spans["wrist_2_joint"]:.3f} rad >= 0.8') + if spans['wrist_3_joint'] >= math.pi: + raise SystemExit( + f'FAIL wrist_3 span {spans["wrist_3_joint"]:.3f} rad >= pi') missing = [phase for phase in REQUIRED_PHASES if not any(phase in text for text in statuses)] if missing: raise SystemExit(f'FAIL status missing {missing}') print( f'PASS motion arm_joints={len(moved)} ' - f'gripper_span={spans["hande_left_finger_joint"]:.4f} phases={len(REQUIRED_PHASES)}') + f'gripper_span={spans["hande_left_finger_joint"]:.4f} ' + f'pan_span={spans["shoulder_pan_joint"]:.4f} ' + f'wrist2_span={spans["wrist_2_joint"]:.4f} ' + f'wrist3_span={spans["wrist_3_joint"]:.4f} ' + f'phases={len(REQUIRED_PHASES)}') node.destroy_node() rclpy.shutdown() diff --git a/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh index bd4e109..ecee279 100755 --- a/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh +++ b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh @@ -8,9 +8,7 @@ mkdir -p /recordings if [[ "${1:-}" == "ros2" && "${2:-}" == "launch" && "${3:-}" == "intrinsic_foxglove_demo" && "${4:-}" == "demo.launch.py" ]]; then shift 4 child=0 - stopping=0 forward() { - stopping=1 if [[ "${child}" -ne 0 ]]; then # The launch process is a session leader. Signal the whole group so # rosbag2 sees SIGINT and writes metadata.yaml plus the MCAP summary. @@ -18,40 +16,24 @@ if [[ "${1:-}" == "ros2" && "${2:-}" == "launch" && "${3:-}" == "intrinsic_foxgl fi } trap forward INT TERM - python3 -c 'import os, sys; os.setsid(); os.execvp(sys.argv[1], sys.argv[1:])' \ + # Bash ignores SIGINT in asynchronous children. Reset it after setsid so + # ros2 launch and rosbag2 still shut down and finalize the MCAP. + python3 -c 'import os, signal, sys; os.setsid(); signal.signal(signal.SIGINT, signal.SIG_DFL); signal.signal(signal.SIGQUIT, signal.SIG_DFL); os.execvp(sys.argv[1], sys.argv[1:])' \ ros2 launch intrinsic_foxglove_demo demo.launch.py \ "record:=${RECORD:-true}" \ "cycles:=${DEMO_CYCLES:-0}" \ "$@" & child=$! - signal_at=0 + # wait returns early when the trap runs. Keep waiting until launch exits so + # rosbag2 can finish the MCAP summary. stop_grace_period is the hard limit. set +e - while true; do - if ! kill -0 "${child}" 2>/dev/null; then - break - fi - if [[ "${stopping}" -eq 1 ]]; then - if [[ "${signal_at}" -eq 0 ]]; then - signal_at=${SECONDS} - fi - if compgen -G "/recordings/intrinsic_grasp_demo_*/metadata.yaml" > /dev/null; then - break - fi - if [[ $((SECONDS - signal_at)) -ge 25 ]]; then - break - fi - fi - sleep 0.3 + status=0 + while kill -0 "${child}" 2>/dev/null; do + wait "${child}" + status=$? done - if kill -0 "${child}" 2>/dev/null; then - kill -TERM -- "-${child}" 2>/dev/null || true - sleep 1 - fi - wait "${child}" - status=$? # Bag finalization can rewrite metadata.yaml as it exits. Chown after that. chown -R "$(stat -c '%u:%g' /recordings)" /recordings || true - set -e exit "${status}" fi From da719689ad964ca297b0349076cd3ffa6d89cdbd Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 28 Sep 2026 22:33:12 +0000 Subject: [PATCH 3/5] Ignore Python bytecode caches Co-authored-by: Mateusz Sadowski --- integrations/ros2/intrinsic_moveit/.gitignore | 1 + .../__pycache__/geometry.cpython-312.pyc | Bin 8880 -> 0 bytes .../grasp_demo_driver.cpython-312.pyc | Bin 78479 -> 0 bytes .../__pycache__/check_motion.cpython-312.pyc | Bin 5758 -> 0 bytes 4 files changed, 1 insertion(+) delete mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/geometry.cpython-312.pyc delete mode 100644 integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/grasp_demo_driver.cpython-312.pyc delete mode 100644 integrations/ros2/intrinsic_moveit/scripts/__pycache__/check_motion.cpython-312.pyc diff --git a/integrations/ros2/intrinsic_moveit/.gitignore b/integrations/ros2/intrinsic_moveit/.gitignore index 4c30283..2ae0b76 100644 --- a/integrations/ros2/intrinsic_moveit/.gitignore +++ b/integrations/ros2/intrinsic_moveit/.gitignore @@ -1,2 +1,3 @@ recordings/* !recordings/.gitkeep +__pycache__/ diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/geometry.cpython-312.pyc b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/__pycache__/geometry.cpython-312.pyc deleted file mode 100644 index 14fce1c095af511adf90589e7e9487d84b04bf58..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8880 zcmeGiTX0)P^{(!{dRVe$CGsOq;@HijQJXlilk`DDiyNi5X_GXhfnXX`)mL`yT2j1M zjvvS%hcJ~}W+Ib-W#~-Rq0Go64DJjcDIb*nFlG4htpXMo&%gjP5WYgp2kjTG7T(?~k06HWQpi{B~bV&|?Rgx2+TXF&PNL2u@2|3EKoeAtcOp1ICmzA_kKrNybHvOWKk)%*UZN$tQWUEF=Y^d`<6Z zLQIO1?G@;aIwih*tXKo|CyoPm(4Us`dM;>ruTV~(fDyGcU^hyoToNSF;H#V`#t$qU zV2Q>xU^OnG7O@P&<^b(Y{t?s1TeN~9St}gRkVlye%Eq&4k^!m}-KB<`km$ijuR`?0hXIP0N-*4fM+-YDv{*Qc~J>U%bz610)B_g z5W;=aCvaojkN6Q1ASVJu6?+Hc;RI-&t!=+bF331qOUuWJeIs%DihMrY6WP}vOGNr; zIME-E$@^$r?!X$_ACvogLecnzNPhxrLnHD&nf8?7dgG&ggMbf7k!XC)`o2g!8cEQx zw)0~jyAJn7Vv*7F^zrfLq2X{Ml!%9-;Y8cv!FW%2P=374#G`;@KY+h-|4eZo`U2UZ zt2?t~X1C%Cd?ce}F4i@WKHuN=PR`B@eL5ig1*%x09>^Hbt8PC*tD(vu_{XD+$la@2#Z0mF+8_u=PMy8{RcZcHI zk?YY$nm={J_s*HQbIPvAZk+yk=Lh8dqaSoGJa_7sN8dlLeEM?>&ePCEhGaUY#VpBo z&Wh93imN%tTeMeN?y-xZeEVC&`Jwstx#71al$JyDvK0%;!^p*Lhfrt#hm)(>_z#!h z@!#eI(Z390=2mPOTp4CTe*tiYD{T`?E6F8LkyyG;jC}~3%d}r*?JX=>Y3mtVa02!_ z*w7#-;Ij#r(f0ra_6UnLL zTopJw;Hq?-y8`FPkXRewJvzP`bH;6?thUCi?f&FBNW9UC`4=Lw{urM`V!dFwa)!`m zt$CrP)@F}0WZXl}W}|s9HhY}m%GsloG|4z(KLae|1M+*DiN?JqaVDX4%w#1Wr4_=} zTZ@0uoF&eLtu5qCCS|j9#J;!=>1$G6&9k`@7Sdl!JCNs<^C`&{*JCYC?dE(OrJa)$ zjgiah>=tzni#oxm>lMH&dQVXBd74RQmYmkW&p!AdEj$sh-9n#s3&WUzjc$XCHfv?9 zH_BK=v0_fA(aVem8$mrQm(yT9^vT+?KJ9cPRH=@zTyhkun4sWBsShx81GaFgP8v@z zhpG}vcVZJ!1`ijZxVLCMf(8VQ0A%!anmcJ}(H#o7CG?HxS7-pxbx3y-SW9tB&Z^5* zQ&pKq-ad5G8Ax?5d3;yzo7p|RJKKKc!Gfnbb+qUJn}6{uOSS$P*R(6UGq-*A!9wk> z{6?jAPg*Q`YBPtXo=uZtbxk@k^@Yq3c#{#Ox;eK~@!WrJ)vuAFt15kPsv*;!d4Brg z4_!^*mHe|K;FS)}HGH^jzrn-y9Leo?%Q3q*-j}pSPkwcQKu0}H|dx&;)g2R){d(DyM9`c)|0&4Ws2@HxSk?Q_%Sm(`h+Wpy@lpF>=Z zN3U;y_w2PDhLMx}X>N``aSI*vE#xP_Yg>M0DQA;i6UG`s!Ev4VSBD!5`Xi}hPz<)q1z#fHt9h+eF3G71KAjnyOYV?)CS zB*}oFqsp?7Nx!0{Z5=Jm-d42a*U=KyhouBwqp$?ra*_bvn!(-`4R2$60 z!s3+X0e5`X6ehn(IG0%VVe*xP(}^X8X=#&SGv-9Nf7L>KR4*uFbWim%3-Ph6Rk$ch zn&}zEIHA&-U`$TYIWfBe?hkz#eawOq1-$CQs76;#2=SQ}dJ>4ffP_B)0BY&>zWxw+ zv--{1r*qQm$wK{}RPfc~lgBTeTz8#sOZJO7nkVpeiGi zTvrbrKRfyCr4z-w7TqkwwI7>9-2mXr{t+s0u@%@)7|qlV*BfIq>E7mL#M;}VO$Rq}j5&@2=# zwdyT94uvj6dT0=8D*|zQf9yhpCg{_E8bUOJ6j$^%W;bM8;rm=R3}3zCZAo<%>$l}> zIUjtTIZ>(Kl{&dp<+;3TYFEbhdLVVAxS?t0f$0ZQN7A;m556bUr{J?sKBFTto3me2 ze4rINzB4UO9xIks$aH$Z0tS<8g&qP);|dKxMP;(S?6sr>SyrYs`b7Iyk=CFz-A;+T zPk^=OH5acvvdOh&zjwn1Y=qy;9qW&9fDdw)g&tcU*JC>enh96Jf}5MRebXUu>AH2&8t_KNgPTQX?!>)v2XywQuOtz2fR1De)aT6;q^+b zX3s$8PXT|b;)U?wa70GS07h5`%mX7)0;=!PD5a3hF!KXeD7Q4*xOa(pVD zO%|McmeyO7X`kxLOcb0uS&Mc3y6ts(?6*0mX#eOA&(%J_BBS>YR0Zon(Ln%4!T_Ib z^DZd7%DqbPHN0eI;VF#kg}b6l&Qdfh@kZ=idNDU@@a>fyFZ5gEKRmJG)d)8%UI{J7 z0DRkKKDF*y`P5n(YYGf@QaAs`7)D-31<@~Rj)6fg^heq@c%&ulkyg81#=i#e;g___ z6EN1ukt+7bq{yf$Nc~ZnIceQSr&8=%i{cI*zaPzXKin;cUJeiTh9aX0I00KcsJ;fo zGCm~#Q+rYMhdwxS_4FHc+0i`7AIdwGZS93k9fi6BKZ)E3{=DwpX9^E>{kpD8@rShM z)~|OQB6_e4mpj8z*bd=XUj)Kv7CxN8?r{ZQd{Mh|^IP#n&jQ61lC}W&2-MwOb9LiQ zf9U4tzqIhexx(ka4CR6^q_{&1LWuci7@!oE`?PB&F*Y1MKc?LW`i%GGP*m=d+W=^x zJDM+cE1<F z1_Xza((XRbz7k)|y!X7<|Ns84_b)Rt(hYFi|Nf5$`~S>f_tDmw%0ZBYRKtr?Fp?-^6~+ehd4x`mOj)8?p`C{dObeF%3C})BWk}**xSNcKKby z8UBpnOn)W|vkbY1J$?^+whm)@ zZK!Ct*k3$c;x8F4^_LEp`OAjO{pBpqKI9v&@K>;B$57>PmA{HTrw>&R*Z6DLvva6+ zxXxe4o?S!BhU@+H>^Wm-`S1$=3ig~i)G*xWZ)DHzp{C(xfAesQzh!u(ejgSwn5ZtNp9lbN0}h;kEv?!|VL(hS&SovoP<_hT)C=jl-M#n}*x{?JO*3X!CG~ zzk@yJ4s99U>fbv2nE$ciZT@Y;o&L_@E`QhXcK`O_9sV7|JN-L{clmb>@AmIz>GOv6 z4Da>t9p2~PhxdGc_h|!H@Hc|60YDCtN#F395f96jxw~`@3_Ta@E=0H zQsg_#mHB(P@-G?uM}mj?jhydGM*rh!h8+g3;!6guGPvSRlbq9kl&eCjCxXYg>a)uX z2Hu`#2tM&z0@F;(=-Y(^D z4TpRC&IGw_qeDXjA=I<~6vayJ)BxwU(UDM??;RKkha}4mzBe=`xp$A^Rf~{3`$nG* z?h1GD{3u$<1w&HCfuY`!k%5uZhx>vfK`H&v=&8{#8Wj%KTcqr*V`D=X7pCy+2!^-y z^5I};pm*dzZ}^OqfslQp)CZK3z2aL3C}8`Z1+TmI;5*YP6>DDzZ)Pe2zpkx)lwsS@ zKyW0C_q;c$u0*Kwei z?;Q??gFHsSL*sL5U}zwG@xbWNK;K0v{ov@~1N>8#h}KER+J zMmFvMKQN3hdOC|B3;ke71Q;eCuB?dAG< zLuibYrH1YuK)?vvXOx`#dik?Kp3RZ^bjdm}@^p|7OF6^6DD*<0pC26#T(~$M_}sbP zu#}ky4UY!KFqJJ1jpsC^dG%5eoxq%@r(1)2lz+rx%RBCxh^2!k*nm}scvcA>9 zRjun=dV5>?TY_ua`dYb_Eo*vP`%kq6Ppxd}JGHuX_4?MnR^yZ{XTlgxY)uA3#PDV!K4J_2bjePO4=aET zDI>-~vywvl9S2b4jI=c>B`LosQB5B)9yZjcVb6sN{`FsPt+(*h2FXN2V+sZPBnwvK z2q)P_dPgwb`Y=Ci_!%AHLXw?JY>6Riq0e`JqWSzNe>ODM+ZUw%2%eUA%uq8w8fs1I zp z`s~hGbIzR8`|SRE$ZY6S2L&(G?Q9VL48sF_!WgC`%nho;q=Xm@)cq4CE^We$KT8;I zvP&91W5hs16)}xF7Z25hHSAFGM68^NrL=P9aRk1ZsISv-Vhw7sVItU23hJQP346pI zNpl)F3*|YBv>_IPcf_9U1iUj|E)Anj*&(%&QYw>V!Vxk5iZ$YhSXf@;$wipaVQu*$B+2Jz5MB5*tf#h`u%6m`e-3@ zLEpFD9wKPq3!mu?`vyY3z9B4u5Z*_9edl-{+hr5qjxxG`Ok?q5nixNN@xTA!)X)BW zW^=tmGGjYFBc+WE@Y@i>cfrGQ>czGqnR-tFc=v{TM_RGW21X=P-)N}bDOu%qNVY`z zl7+PZ;pYJg1|;(-)Gs;64{+EOB`e##&Przb3d!_vBvbpZwg_!KvEml{c2hJ&m6jY?jifdCrqJnKNa* zUMhHMqn0_R=StU=HItoJ+wVAiQ(evHyPl2AdvdO&Urm3d?bUVH*G+AoJ}OkV#fw+J z6aJ$MZ(q1&y`3*?IS^laFzz{odfj=GxtE?{#a^j-Vbh$;b7lLb?r7&-tHI@YcF$dl zAv62Rkt^F@IH5+-SMTBXpilbnr#c#Ja`%1>9rZl6fuMno68^NP0SKq@OLiP2(tui; zITL&fXNGU(Ebwid6~3Lb!FOBGdLIgOfCbyo6Cgn;o3Pj(AX@_ zgSTuh3%-}jhM&WE;pcKW@bkD_`1xEO`~of?ej!%?zlbY@U(6N3FX4*emvSZW%eYeb zXdt_FS`R||g`R|mhITLynQR}X&$ zw;X;0w*r16*8snXYlPp-HNkJ;n&GeHTHpgifWL}sh2O@lg1?$;gTIDb4Sy}y!L31C z)^Tg`ww_xDe*?E3{zh&C{7u|O`0d;#_?x4K3r2qjVAvMP%}~uw0N+jq5f%Vnb>F^% z{q(CwfM0nC;a^IN7+%D#$r+Ws&uGwg>NgYVB8C7hZ|1Z;c@R5hs!Pg2AfLGbfa!n= zvXk=b>~=}{)xDoX8v|-x?9Hi^9WGR!28)y@?UyqtUxgCXo%ol?<5tSz=o{4bvbU@S zaoNh-X-er`@Rp;zv3$Aen;9wd7QA8SmR-a!xKer6ewE%aqK?5;TDbD0#fMibVVorl z2r0Y4b;^_W8{VKi4{lPPwBNU^%879TuzHYSMv4n>Q35zy#HggkmsKd?56uVYo0PBe zA^El|@tk8pzG@|WiTZXc@f;v$Qhl{b_(StK7v!sZNWKF~Jm=yvlIp8h!k4H|%ZF_+ zDc|yk~bGb+{qP%IpTn56`T}}&sEAPz&uFKY`UfJ4Q z%tskxT3+P|xZOj`4u_P~%DTs@Q&)5Nyb_C&v<+Vg#15ihlE|g5{P5?M1WAW5LtwKO zI{1w8to?EYsN-4fU3psa8xqij^^ab8g~<7wQi}HbNag*imR@-Z|C;i|6+NsB^ajoc zvO`+E(q2&Fx#CACZM%|siPHXEQv4O=S^Is28VIRnZiyPcqQrA0k5JkkCG`@eO)BwR z=_8c3Pf5K*X|E{pT-hU(wqHpdzM(ut4B_8Vp1JZR%P`AnFuI47)CEEM6Hk#K1O8GAQuStje#l^J~Qr06au2zIZDM7 zp%kQsP#=iCjE;6bcxpT=xl|&*g?^llMvmvm;bS}zKR}{jQA%hiI0TX|Dr0fu1&Nv# zga(E~r^lUgOy9-6q2PF?{Lp>990<7Qf z7dNN0jN;`l4&Bdh?i(E*1MwSeO^}vZuitm75d&wFUi#18_cjM*dU*iE%LKvven~Tt z>H?rcg?SLngFGr_B>6z!FjxP$WC@)EeO|H$0s|uh;Xr^VBv~>A`c6rXFrz4=!IF9J zXx~}M5*VT%n@mB(vApYP9ET+r$b%q5v2F^1w$z&-&`Zuh7}U~{-cv&$@TZN8Nv7b% zAd>e4K>Y&YboA7ql#Vx=QS_V<7z=`wiVmX5fb6HwjN<0yFAcT;M zA<2r*9RS_l5m080WMT6`vMVz|@?u){4fXOt^kIS$9+EuxoL(*|gfGLl^X1e5_CsBJ zcWr%aZx>%mukM~hTe}bM>gw(Z?A^a@>t4yz2f8H&XYAZ5Fd@zac^-@rgXEA`6hfs8 zbp=r(p0ITgqOoq+D2rJe$b9(@;_foA(@mSiA~^2)Epid##D7u z`>4|~yQEC@h4nmoT^^S{#x>#T+$A~X)yER>bcT|gNh^(~^B4FAj9tT)DCQCW^gtgb zU{WAYhc@(_vKVxiTeTTu z)Gyi4)5O%mEDm6zhC+crh!7v2?^#BN;-ALLc)vRN*>U&`^ zDKHof)v*rtQA!~S^TldSmC3bC|RC~cj0 z<;MzF&aAy>GL~&(&HJdJL@a0&3L4+yVr_eGpN1{){2#D zh03+xbi}qDi#>TFUU`xt%I7L-#ELaS#hQ0MAM5nTj-QNI1Sq0x&Q~q^Rtvt>?*wD* zM`OpHjQdy}rRb`}$DNxQzI7;GxRZUF+N7G;`dznM<0aj4i?r0SHBZJ)1mcBHvGm2| zVsVR5-11I-Yy+#enAKaLcKW<4Z_+)TBi3&g>Nnp?iyb~H9)40d{N!De!BO$4!RR<) zOiG&_%UL#EEiUgAmUrGdjlm`&BB+3rG|f@-sohZIdzpqhbH2D?y65XB-#B@*Ctmy* zHL_s7taYX>Ubc>2irGjP3iiUu0_t&xLy&utcH44Rz^RBX(Z|!aK_w66p(d3e*dj@04AtS3VDN$amVD&BTduQH1 zqb54AC{f{Ln^?R`C|-4|^Lsnr->IhTPNbwhULh2&n0J-L%CtI*8c=5u)p20}zrTuKe*e`5^3tC59^CtxDTU%yD>v*>V$u5MrN@}FMvN~T zemm_Ubv<|-R+2GgaSyQ zLq?&c^zi8D5IFmY=YJ1vmx>nQ4gtcCh=n8NYYb$WJqqM@7|f@7msHF%9#tT8hB)M> zh#wmYTnqPpF@ZM$$A6`w-`Ma2JG*XiWL+&0sj?dDT~ZR z9egjok_rh7O9f0)0I2iRL14!assMK13tsqXsYpc@7RR}iDn^0wN}0+VV>WS8p7K62 zibg~30AehoXCd97gb(*#2vAO{reLi19Qg95g5mQJ5DAQ(K_`W%sr5F=nHUCcU^pb@ zCSDjLj0rwKjN&{xpAhswXkZ+;x%>>d1rpEjF6hogVBUhjL?2(cC>7HAVE}DS)NCI*1LGfmlXk%Nw5%!O4p5L%_P`y=!L`zcCNV_B2ew#Y&nb`q8Q6((6Yh4((H6606IV6r=XZL+i>=ay_Ij6~cw-pvv4} zweqC>4yue7Ei9}uRhIbP%tvX1k_O+cGU=8m&#k1@`zGY4BPnxBLFIHBUNZHi zoi_BPoggt8lQyR!>aKG&?Xug96cZ-2pWv0;#}Au*c@L^ri9?A*my$~)8Y6RPmANl% zfLgU&7T$SiZ{ls#gJMCFr9eAp^1 zf5Z~ieSV3S`5sgbX5TWUMN8zbcu@We#*h`FFemul@BUq!pc=etWO*G?1**n zh|(VIH)46KTFF0Rj9Ag?6-q4XJElBqzY6PU@JTI9dE#o2M}?wFD1>`zUp8}fh*L!b zv^eY(DwYJXf9x|xw!fWMp3LVB^~<^!$P*D_L=@J0JUo&ox|F7O;MF+DdRq`SHe=Fu zYt*EUAZ3v_KC9a;1a%V)GoClRVeH27BLfnP8l9gHped52pJdkSP5j^BjRmn&zyhkP zWaCCbYX&Q6<)TAC3h$t46Hb4NUi!K@;)H%FW}XiegvO6{$u>e z-e1zwUy=9MPM-<;#shur z_f1W0{o`&Q&h@?{hdQ_W=veBjZ{q)!ilZS)?E}CxgJeC#r~rH`y_irCuXccxkvN(r zPO8eQd`9K}j&fuWHGl;$nn0#XA>Zl9LPe_d>lQz>l2fx3Nm;X{p|n zT*>5tq;8$3tj?r^t>jh+2MWC)OQRPgg7kV{a>?gq@JvATkR0;qAIE)}a*}aiS9f<; zXJFg@efxIxNLJFQfu4k2C7-0zS{NEV4Uv7xJTTHfT5n?$pZ|Mmp_%9ca__Ts@n2aW z4Nb^~$ERfRSow7PA_B&J4_g>CAw&OYV(Z}3G^3-N4y8oIC#vf7o_OIpv2e3cxS1%a zjQHLplr&9$AzrdsEa?p(AX2NKN9y6Tf)xR5)DF8!}RfZ(FU<-i%_(M8qU}ZZ9-*RZ1t{q2_UDJZ;HFxMb|dL zwGE7uhAxnoXYI>AE~%KR7AxgiyW*AG#Y(6E?2VV~qv*o9m8->-+k}+6{}|U$16I;irqrR?s(}QDx>J*BY}T5 zrIn=5?K>Fl{7TKFSu9vC6fB=UezW>!UTnpdc)?b&V24n!Bc8oeaP9nr(o)8PPpt-D z%~X$AvsS2C8(ViMUUOKiIVRK`iv(T_P)^Rl6@PycKQfN3CuVsvus`;vA#9i4a)NcfnDqep`tbanNeM%<5;Lix&A>)v?zKC%3;P<}XG*n?@1 zIagJuOv*j+s=Z>>A))FJD|Id_SInvsvZ`X$?eVP5VpgY+)p;lD$X{hdt#fX#=&lgl z6|u_Aad(I4?h@Qxknmo!t}A9nGQ{zoaq2xrv3f)Tg@@op`U`_(_{SXlPW{uQT&)OKp`S7d{C8rxXSz& zP0fdw;Z%YE#Suz?{lIYVU*KIM@l+)y>36jO7!{>rPG55ep;T6iQ>ZeaedCOD95qki z45o<^YbP|+oSu>yuaUG(@GqB}4GvOe#XqG)CZeO#O|+bYDt$u>Q*bZBld23W4I$%s zU`xmFRp5?PUoagroHz0xA$`4_AIA@1*}xdXMVPu8(-mYOi~l@g`3aUJ6b#p!Wn!fX z9DNye`Co`)cmwUt^3MD!f^>G~6fXTQC}Tf7ASfAGFZnO|e*&`N>iNo=sXiL%8PgxR z-gd<++d=!wSba6~iuuZ!`I_a^>%JDaV!r0O>YD7iqgsxNkPzsO^PDmbC?uHy-6{sP6(txA0f+xor_`Z0DY&#SNhB}_ z@*%#I+{@6H*8c{?n15R(KhjjHw{(x!KyB=7@9E%%=7i+YnO>edPr@xpvRBRHl?gG4 zAm>{wp+xKy-?H%>->Hj`YHIMw$eN6U-8a^gP7X90dnFqmWO6tB&(To6n!K;VlWd8? z`OhfmJ_VVe8@e)^?rbyJ9s*fOSXX!n2Y`Wd;q#Zm)8G{+%T zyB~c5o1Hv_Ggshr)R$bIOUr{sF|N7Y*XTLTauV{-}=Tor_9W;jsPz5H+{c_| z{WZW$)MGEjm_hEFQltU%hs2Fzl~!el$^Gx-}x zBYB2KN6(%cBZ*X58J|%fpxoIv1U+OkaoxejCEuYXSxGtV3Wu1d4M{Rykc<~41vahy;8_tIa7DD?Y)k7JN~Nc`@28b{r&wP?2qp~5zjsuwapb(h(#-eq7`D%TA^rd z)IFD9Cgv{_@|TJED~0@(QP;e?YA&Nh%qSBw%BI|+Z>8W{InyDo>J(OW##~)<8HF#M zymWH%_^T(cpPW9<I;eD~mRosJmew<-anwCw9!Yx! z3gOyjtHe)OkY7cll-!yq{h+#VbmCNQ-Qu{2MQe zMz}%^RcWb+SHA`&R4Xa8Up+o%WC3Eh=#24)GPY1<2&?U{Ht;4ogIqM6S8#~}O!N92 zhD-3zpg#T!hiBqWHcKo;CIGI}05*D$DoWoJDD zTMi&A5eyn0gZMQG^gtSj5%M56e}=4OsDTYsei_Q=zea^-gdiByqihrPR{5A?SB1$P zn#{N@AuulIu0|0IUeNhRIRX7HiX1Oqw7fORVX8RvPe_%7>R24LbKZQ>TXn}<1zqf) zSrBm7K%Ui5P;%`HSHBS52>>V-)C&dm(Oq+SMb}2Jjz)L<%!xEVaT#1WchgK6`JiGY zWNK$@Ldn{zj=Oe4VP$mJe0F6#yBZrsdDW|5xc-HBd9zr)N+@3yFJCQ|ZxTR)Dc>Bm zy_9(=lS!4sKre-=#Gh!d7wq-ZbsySWe^#Tk`WK&O z8*)nN@Rt8WPab4EGVfsDEB%HF&AB5yRX#ZeGLUq40B^W@|Z>Cxq$&VuRkkfwVe+_!;Ay38a+mnK}S<5LAm)ryUk`=?QNnP6li>yXDpV zt0;qSBX2pau@&SskjFR+jH$rb3x7#T{))UGllT9Uca=P_w7_zZm!XAeV!#dq8E#6j zND{P$e~+}|WlOF!CUp;kS%%PSNSL(PjEt3!iD?Mtt zTL7t9T4pk3Yx3NO_L`rSqW-%$(bU{$b|7BN*p_DccA90IbBROE$M~A1)`A`as?B+g z9Dy04Lv~P(5^t%jEhU7J91^n!)vNB8s845AD5dMwcacy!oGi^W7i|1*p-MI{_$qqZ zM&1kLy-eP(ljowL#$VzL%D;`r@ros)xmk?+*?)C+}8)+7<{vN zwtT~^ePhbO{CD!&+p zWvXQPMX1VvV#qYtprE%H8fuX{srxseEV=uW3BQx!S$lPYaw6Kx1$+5x*6Ge!-^y8g zD;o=v82r%glY2e6XH>kDXh+XmpSL9w0rW6Y<;!2N4j5jrMXga=e;Qc{fq@#M9%(?^ z6@d|2^hO|};#fw*C957h(Fl5>5>2{K|Ck1H9FF^(2^?~>#tDDU$XPDgz_*9bS%?_$ zoDok}dV;+T+d_Ugbr9o+e+ds?O}hpnGp4}up^6XXV}=cbb> za;H)5V*tPt#xEEzSIfWW!5f!>#v7(?$qH>r@Hiz4^w>uEH%$CiGzhlX;DKBX+5-59 z=U_Jn=6He^`al{Sg<(AFnNhM(mN;HLE9)siIS}T+pqw>cyJ*dm-lWDRPoz7D@$qaQazXI;fO~;Bz4Ow}7kB^sQ%eG&shz8=LHzmObpL|z zn%evK2!wypTW5IP(P5ggY%u+?rNcCy<)Z^8=L7Z8*V_+~W*@lDEBhs5h<}Vejy@CH z+6Cn0Bk-hjYGa@`baA9l%If8~0DUPmFlB{L$$Y*SELRtuS5YLh83?gu{%I!J4nqDn zs0^#Ddq$+w&z&P9L^f&!((|;+c-ras?d0u%$H-@dw)%WqwlLy7v@EH<@m$^hgXu>@ z=!>Z4GqTlH@X|Avo{77mtTmVAjaq+dwm9mot8!{>JZlA*KXyPm(zTk( zpS9P674*`@OBW~eXWbR^*=19;uQ%Ulj#aeATDRTWaeLdX_Sk`gvGPOl?87nF;W=ma zm43kqoveb%{Hx&hRLr?^ubdIw zdWYVsPg?0%APNCPfrKGvSUu5cW?`_-&6k{PojW;mVx2qX6+dBx%#IFjiIUWl9cNX_ zf6ka>#w!_4VO+_n`(QeH#1&1Cy3ivw?1MJc@_@4~nKIMe1yw0x?1Jff{gJc_CV#ss zW6ZwY#lGF8e!GivP%r3V#Mt-J|1aP7>S^C2-Dv1Uv&~=|<}64{Y<+g&S+e)`(Xltk z$t#t1Zm44>_Bp6ICt}AN7p+t}$_7r4_9kz4N^*`?@WVE|+?V!V+WTVn12#N<7s~k& z%@%{9ZOeF#w$afZ1-%d-BtLxA2A^Cr>AT1udK5XwGguTKB!!(Ix8S_@e;tV8x1h!R z;_;Co4QZ6f@GGzcM3Q62S3J@<$%!w}D0QH36JV)vZh6bs8{cSrv-wKK4?MLDE;Zg~!!%y;fYFiblR=R)2^6{2NhHG~p0<9z8=ho>QV_qNUZ0>> z*C8lv1H{Pi$V0@LM1L?qCKsK-fD=#PRx;Dr^M|Q`WAyH1Rml*Zr`f@uq+q326A@Gw zf1Er5T>J@mA=)Mhb}_dFP6lD=z&jHwK!?Vv#yv$v{4+Uo>+-83jLN&_a3&bn(@&5y;ix5ZrBsODk8Sv8kkK4lZKm&IJm=G-Nd zPYCW>(cLJx8)Npy#pe{2Wd`-hB~O=}v7e}|I&gzaV==UTQykLkP;FUiof9Z$Vlg@= zP0q|>bWV|+r9bVomC-bG;2>vZ`E=+k9B};8B%h(eH9dHuKtveB#FxQHs2xT^RlNs2 zI*XFyijjZv5z988MV})5wn!Qne%6<4eI_R14=N6mBC#^~Tb$Chcvb3xq0sz@4J^PS zGt7!kP^QK7pY2%3Tv}~r;`LFl_C+E_o zoU0m5G@BHx3@3XBm%-+jCFyII>GX`MAp{wvN1BCvVN!`Y6hvj5B5V%wX;+Mww}n?I zHFK&4(Q^=khRija4+*L3#B;r;n;$_2ew4g1@;*l%PhN<;FnJ%t8*fsyUSYl!ha#qQ z=>sO@V@zWoV+@CU5Ge66?O)YzaJn9EosF)}<{O^HEO77zzf*7+kG)$73iIOeRXG{i4ftU0v!%I(q;$eYD z84Vrbt&|kcqu9{*;F0~=EHR^0$S94KuZg+VLY7N;-4t`RtFI(fxK=1%8*{BAaW8Nh zW2K4oFCD#f^vZC|RZS7f^W6+vPI}aSHyh?&Upjv2c&xZ3=2}T9*vtBuYr|cOiL7$K z0z^*1war&I$Gufkrv+~#K)5$AYM;;azI5i&nHL9N=^>V8ytHw~9xq)t*RoE`^WShy zS*Ojntm2lV!j_{SZaMZF;aHwOxMBmdQl()!VHCw(OR(G$tue#%&3ei&w6S}dwqqCkTz{r`G8MyK6%ILN^ zSk683!ZUNttHkClLh}}}d56%vW43wM^|IUHD^@UZ-_D!rzwyM)Ly)_@VoNP^>Y1B4 z?-jmVcx!EZ{T`ui&#Y(fTyfcy_xk!P_Ic2tcVF6lC3JcJr-cT04t@LO|UWE|4HL#|2h9yhCphJ#m%Vrq?&f zOLe6<1(ZzKGy;QWEf)f*-ysmL5f@O34zi@j+K!ka%sx5$MXjkMiwVcSs#J5t$}FYR zuU@I?|Ef}LPFPr9)Iw(@jc-ZnRXz3^th91zL{wm{g6&m~Nmnro=m~@ueWn9I3FQbG zFTagg6(DPa1EeH9;#5`vfzZPSvq7`%t0{j!XM}|1kC!1Uv!}V zU~CTseZ4*=73rf{tJpvfpCNcj*2No{{R!Xs0oXh^U!Rf0SUZ5?Kp5=ha8pQ#|3|9k z5P5$`-v3~c=SBudXG5}_xi~f&mNil$w7`1dB>)rXl(JQIT!bNcf>7e%VH!=fYLV7= zK}y5D9%-lWdkW_MN$Eva7cmBSb+z)Jr}aj-JOGjEWQgPn$Y06~is`J{q}XXH>pXzO-m7~RWq^228-zBq@~-e-F8l;s_O|F&vW(7MJ9G7nm{%+0 z)lTu#kKH&wlQwhcZCgBVUEH(&(UN47xT5{KT`X8ew%6;Yi)N0+3p(Q2TcX>3=&gD! z{QCHf@tKO5!CP5x4-24s9=u%?>pAj4snA7w08;bD zc>bz8o>h0j?{E9m#dh*z@l1cr?w7YBb<03+sN4@L+ln-bpMvX6SeQIXVp=1I>NLv+ zn9g)2OmRvHdXzjh&Y6-4uSy(vC3>~0AOL{Gq8Z`FWdIgUY!Ms~iymiOZ&3!+HfT#t zX#+M`f_Tj7iIqjHnmqw_9)q~Js&8!zmn(U((O~|OQxyO~CDqJtqqUWCe6p{1-ifI6 zS)2qcgSE-&v(SbYiNeg?u= z)5nzSdSGw`_V7TSk=$w+Tje2s1xiV9F=UY)NEZ)>WGR~$sWm15YiOJ)MKL|GBo2mx z&St_QPvUMS=-OS-kq%f8{|<@{{VD1O*v~ADXO=^VyR0T^dMW)1o%PNA@w{@e^A~rllz)wD)_suJLMfw2T9)8oihKKOwvt)%Crn2qMZzDiS|mt zUisSQnfiFurdfMCkjZHJXP?*%zAduI>bI)0w{2di?V_S<)A+()S=}p;Bu(S2+tWKCQ$D&Q~em0@nSv8r9Ltm8RWn@ zBj$!=JCtB_S!5H7J;=z;bhrTL9EjwhkPN$I%@v-ci1BD;WLJyX4MKK9Ji7^kRRyI{8%E!oe{I9n4U?Q$)+CfQ&Dd_{#J!tw zCUJUW&f=+cV%4fURjU}aZc{wFoz6(^{HS{|JBvD(VUu@KhE5$OGq!MZk_5Y1PiRsP zf7IK$F`Q0Rl;RjxZ_qQx{<8elfa{{;`p>r)+MtS78RG)Q(D3ah3tVUSEo*Ip!s z1H3W4^=S&IcmjmAb!e%2L#%%&STNu!1yyM&>9T1llmSmq8gK@IF5}anSKYO5|KnXe z?ej8_x#*);pU;4Y0#8JBH)DizRgzAnssg&tpNDFQlQ}ME}I>t(0 zBb_$Fx>ledU!rsH)3B_tdN|UO`#wEsC4HPd8sP3 z@6`vrnVHQy5HD*8IM{V^WRZ=}!Z1nBp8ZF*?F{VQ)xGC1Oa_mh>pR14oD0dAidEjK zc$&*HUiCZl#AdU)zrhS0Gf~b6oKle@TAYNfEZFd9+CvuR8`P#&Dvj-HG<&eG$-9~@ zVeEQ5f6*M5OP7!@^gOa)T6>qry$t|Er4_FRt_PxaQZg^EdTmddZyWrXQOX zdMT3kY;_;h<}K#9&YlfyJ1q&@)hxvY*qKsGb84}9M!8S-G zU`ON?lJWnd7?L_+(g`VOgdJvK*&PKs2G0{QKcPwd8z=;m*j*~RD+PCD++7WeAikQZ zGhcH@%`dqwxn}L9Fq@$~mxCU4t^8^^h{`25N~XgzJ@Kp!Bp#P)>7Z=#!nA2}WO~;- zr*HP(8i_sbkHfz9>f?|PKVdAGFDM7+B7LSaUa&rvzk%crH+>5I_jZVcc=Dndi&0EM z71-qdO*|w|Ze?OXx0T|V3!9ZE6`e@aLoFBSq!R38j*wBJp(^>4bW+o_%{P*!k|s>& z6I6aH??XNYaQS(9`Wy0SxEJV{I%Iy}aPJtOLqkZZQid>BQ9Y(xGvqsakz7aLJBhWZ zCi?{q?nV?!xFAzIozE?NWjR@{w_Hx2Z{P8Q!ksfcuhanFufE~_u&Ct@45eFeo>w$T zi*^~=`Q!E6tyc4bYe{JZv!VD;czB+!w?kdBiyHgcorIbYZKp~xQ@WBjf)lRpTk;Sv zdwXj^*vDC0+mAFU|4S*up~YaQQU^_G1#otgnj=w@QoBM@hb6xCvXXJ~5aM>)1!7$E znXB-3uNK}{je876%)N-=1OOpRvjG#mp=a<8cA#$VGWFoIwu)7Hv`Qz(C4mAo$8A!(}{t zHBFriSY^0P8DX-E7-{3sHls^`PAo))jSxduPd-X6%vlJU%PEyPsx&3}@>;rbH!j_yLRmCIm{nM+49Eo9gHu=`W6i{+m2|g!BEE(!@co|Z6_g>$rEX4 zym`@lOl&fJqkK?jGN#j*Z%3)}WW?BbX@Eb)02fVGgRbxzxZ2a)jp28KH+Q_Z=iNQG z&9_ejD8TN-rsMH7CxlwM^5H|*$)A{z^E0N4_(j+`F&l9Qv?+9Mm`&e=Di!~MGGFEO z^wd)d!zHPaIt-VjSO>MY6z%E56LNCpUy4=CcA1Qz@+J3 zV4C|ratbAD;A}$pmkg)tP`4MvyJ@ugQ3M7nuW+(%3RjM_zH{W}+FKpM#>4T|Jwg#A z8;?Yt^EvsGwwSL?C|G^dEUw!rtlJsebwXHoQYbhX&j~~wbMB&7xK~eKKRs=_J}kIb z+;Q)oaeTAqy`%3Qy%l=*gwUe##8Ic}2$f~~35OxV#Sbut#2Gsw=yqZ*A=n09xHhB@ zM7T-KMrWWQ{;q!jv}%|OIM-GU2?tZjs^KDFDp@tV96*V!N$e+m^-QZ~4jXlQ)UQ<^ zP=R@i^O~ zvZKOvN?{Ru5|c?4%HyDf#946XYGP|vmKfkUkOL?RELZYtzj_QTje%yAb&(pi^vVVIU7p`oO-?^>~u=5?_st zaDv1pAJhk{mDJiV>jC?M9)R=&zBI|;0MjRPe!+Pq?Q#uv6g+7fW%XIz^=SZoWn%+F z;}B}&&f)qBx)~zrI;|0y2{0Y#Ie=tl+;cZ18GcOjm?urR)Bp0uPwwnHbwIv74=7-| z4Ez`|kPeP8ojD4v4bo@`^qvB}IXM9LjgU!#^UTudkL0h~yyd>p2V<_XDbH~)NpFn! zNX;t2%G%%?&ta9wF=GQ`xHxen*p3r26Il%M?f0!XJafVJ`*xg-A*mN?A1{)#$e$(W zlsRL*Wn^fX^THd(5~^X$j3{m+OH@Y}r=%z9qvu?@D1*I2nvf`s!Qrtm z?(smoN6+JOL^XD^4Cu~5H9-$bmZdwHR^ewz zO4@Y%h?J8wD@bpFF$AS_riq9mIOrTc-HR(^P*vhiL&+mIASuKu4+*0+@HVQ84tkR9 z@R4oXx(*+f`Hz`0Dc4IM>A)RpuwjAX`Hv|1-;&3el2*Ark~6va>c{i{4<%&kL1vcw zf8aetl@WrVIT=;($oI*~&80rkk?W*EOdOUeMGm10W$++PlT#3NKqqn;u)1mowA`zr zdnSt@Y?)sz=B*I&R>bofqdVquOT^q3A-5&E9rud8?!MuUZl8rhd_@EO;;ymf^7{(* zzHDwq6TL5EwmZ9om0k4KG`D)axO$hcdKbO5vWv@Fg^E_P=0t_e7uCGn7~Mmb0nf_U zcHMM}>-Gxk_QossePXa>cxCH=bDm<+(x$0%~zqssE zr*%#CPYu>A?_Coz-t`zVyrQcLUnjbj3$EqU8)sc>@1~hE3W@Q*;ra&Urm&kfSWhnA ze#H)R8aXARw?*)_h~ABYccbX-5WF3@;3T(H%xx8NTgBXVA-7%3?G|#o#oQx8?h$m3 zY#|`-DwQumnai%7`ux=J%=(-DJMBHU*T*&;0e$CjV*xCDTzPu(iP!q3Yrl4O>ICRJ zS#6;2WRQJBPtGjeNY^anH{WqLzrB4fzg*036Y|@{{0-e=;X-|>|0Xi58Ci(_ZA<$LQ_>|AcSz1)K51%?%A z1vB*THbeh_tRNuOB@>`PmC!-)6!e0q93i5DsHZ?p?u9d+?ZoP6*Yw_#g&*l|0w%!& zAP9t$+QnRwh@n}x)HFJM(AiNw8z?PNenH+^4t)y?>;;u-Cs(ZvdSxnkm0xI+5SNI# zHxs|G#||V_54Bu?da6|0L^k{=B|He}9Hiq_ic|6}QL9Rmj#wD_4g>=lCZ>puih@|F zil*uzsY=-s4t$m4Az!8Y)w)tz`Xy;CH4OSvJ`%5|jS+`t@kkpp%)IKwsg*H8tBI18 z;s!CBQk`}q3-nC#cXur@p zv8ZUYGva(yG&-!J(Stfj@>v`@LN1i1gDi&#m8`%l+oH8XdD26Q^-Ga094Mn>#DVhi z9(M51DV3+o#IZM|y1^Z(A)qBCaWpQ1Fqtrth`R+6Pw9}PvvvQNPGU?x_q1F_`xYj* zkU;sWw=efs-Ukcfv0GjlA}6z_Gy2am_A}G` zR}C|=3&dnT9bp+V&h$P#{T)@qm>vuh`U86XM~e9W$RqrLzl$fyMFM_gv4a`5v(Q@t zyZSPvu_BG4obul(C;K@1F3C6m!;vE#Zk`Q6Oc3`BOBVJjp%CC`47rgFyAuktgxIfk zLoJ!uVz+!RZM#KD(oQJLED8n6Qd}bzuMvvZ#BlRxR&Fe>R?KS@@*3k=O;PI)-MMqF zY*1CBPyhPH$?QAM;`v?OxB0&;{6XH~TU);pdS&%gmJD@gw%!=Uy{R{IgbG-F-~3^I zM=b9!sfHZ>IIrlH)vtM{9{*Z#ytpx**A(3`@Am%s)3AV0PK+K%eGn%d7Pz{v!_0ew zSkfYtw8TqV#gg?x$@+K+OufsA3V}G?GdLaVqFr-2MU$(q?u&NK=T#+RR<%H{$QseC zs|sefU*C9Rqgd4@RJAFgzPXzE*8?{KvFg>)9kZ^A`BgAPzEfP+BdqI*uR0RlA-YH` zNp!6cTq|Z>EpvyCiidiIL%ly(d+K)XH!U}Vf9krk_Ec)RHV7SG8^1a}RsDM1jk@Wz z;56m0hvhMMzRc$m-L+67on5{`T>hA_{INUk$8IkZcL(n54*bwlKxht>P(&A5MVdS# zmaP)XR?Tb`%GUegu5#-Fb_%gC7;-U-Ik2=X6OB2GGqORC>7jzBB*Z{ZG)hfp z0hcRTNLiQhin_AV!W0*=fGtn0h9kobzmkWoP)DagPjrB(C)ytJ4fX*8^f6Rm016Pc z{fTsqwgj-G0&BS)`*uK|UiTnadP&T%?lXQwy+A=^?*15E8eY9 zxj4|tP*Qp_Wg}uGlgJv^1JsR1Afpeo8LY>u0Q(;l9_`92GKdHYhUp6?0I>b?QxYx1 z**#v~v+aP7_Cnv&1EF)c+jE>9o>kI;d=E=E`e6b8&5Qt|yxePrr!riCW`f~@%~E#S z%1AIqvKBH%jEsH_3r~#y5kRJ715Yx)W6dIdN80RLlj1v7NaZcgX_kX{*NV^RcPrz9GjoTpa?v56}LI|#iOBrB)`FtZ~c z{Z&AktSuv#N4lKy!QFZ)2swqICVs$wN~Kwtg&@f)6a5+NmTZadTLN5q1B*Xc=f2Rz ziJqXoCxZ&i=#gWu5qojsZq5xjj^m_>JEM~fEW3T8yGC%=#NBm(bVZPJz=a?w(hNuA zS?!Q!Sk@FU#lG90ix(e^Zoks@!d{$*AyfNtb}pvLbpO|f z-xz*#bT)T$JR7zvQ$;DJN~goq{WE)RS#Msrbuo7EiP*8@u>&V!|cd zR|wt}cbqFo9=l@G&7Pavaa1nvjAwU|NnCesG~?%=mK!`!5i-jCM0WnvQ+%wZ^VZ>8 zU9l=aygheZd&uktlHCsxz5lta&XtDmtaNnMnZC23va8bc-AW6d7aWi27|S5oDLgD? z8Vh$*>ZAB}nvGU6dx_yM~U*$;>3scld_mf@@D~{ z$X$_*(1rX#QLj8}ze+EnER}YFl#N;nL0;MyTP2OG!CQb0Rzp>_T@w9fwv zt@8?|W5v@L%7@8DifSfLNBc{@W$ceBF$s!Fj`O|H2~`R1lAWMaC>$J<%%^aHy5wOE z(8QTT{5{J1F%|z`$s_tcPx7BUJB7YQu|zG0jf2slbIicAdZ0|Qk3fc;+4pFm7`i{- zpxkAY+rsMQ{~Z-fG<2Cr&M4!wy(XyQGD$oG*6ny)IRXXPKzMY#bP?{796d^VS_eub zK?mq!#ofyw;g?-0W-k}Am&dak0CFmm>?A*uVdaGQHLUj(B`h^3Y$>}H`DuCUqzgle zeH2$PUsX44`PxQ$_OZMCI)$cAdRsBqx<+i>DYWjSx8}LBO0le2C~J(P(0pl-e2684H^u2e06m%T&?+d0uf)n%_gJkUb>-_o z1RR*qesb(!nRu2%WFHbL>LKX?-*6nWcGJJ@8yJmC2QulGsZ4m+8CFI#Y+ zQoj{S>elYl{mlJS$M`yR$*3ET4-CQv(njYmk}0_J130{>YF#1U!08bnz})un0xc^$ z;3x~-$2KnLhUL@#l}jEmrZjSsygM-DouM+l=z2vWkM;yUkCHP=6HN=+0_63dt)k3o z)#H_mHUR}=R?~bD#fAPON&Coq^<@B>S7haxv=NQ>E0W zOj6XTA|~=gC0`GbN|L#OjFEatHgW@XB%GYN2T3SDa!snt4z>+F)Jy*=hGcPlGY~nV zBB!`PUrGR8!<9;y9*ZJDY&YxCHbPZNbPdy^*9bJkynz0pTe@t1R8Q7tfoQNbp?Llu zur$ZLklY_TM`GD$7oN^#ig{iVty@D+7#8vwt!ze0&!#7)kfIT~{|;UB2sCb<#Jkz~ zoM8-#4k?{|WbA=4+u7DZP>QyW#NJ^?i)Dip1&el!M9hn{Py8dY0b|H67qjbx?7Db1 zP=p0|0`5~Nzfm6ZF(6$!-@IyO*B^Di-5qc1j5l{hZHlrtS^1lVe(BMtez2{v;1TPtkC-TOUFkV;FQCavPRjH&vaE`4gF~#XY z^CeQ!`7*^Vky2gSTJj}Qs^=*!Wxs0kn{J)>Rcx94Ob(UVCR8DOs~@eEPayI!&Dhvicc0s!)1Y3_WSUm<2iKiu#b%01_R*Q_%2)= zZ(k;#O$IPG#vS#(Fb)ub@pdqmim`LIXr)rDfYf0R!+9|em3T^PW3vJtPo5h_^2s@t;clw2$ejvzY$Bp+4 z#_SVD>BPy{Q@ybhy|~CSyU!T4lDc@Z0sOg4Tm(!e$zSid(J`|sR=IYDzghiW6%ZXf z^@K~EfcoVPDEIaBVd@UxLQ34kUbSfJUz}))rcNiyPtd}O1bdNe zyJNPBvAveMVr069-MaMz?m({IVf?AV=-8RK{xSLRM8qVvu^zxYPsZI8L3%wLPK`4o zcZyRES%fh`Ovc1iim+9wmdZ>?LM-a`1~nir6MT_vndzw#5vL1%L5?_GB(6^sl8E)$ z9IZsn#5v;46c190k+aehy9msQhZKjh)RL84_+_Ff5ZniP0IW>sz?PCPQ3qq}<0~eh ziB@liUG6blW;$?|tkwi&*|?Bo!d>OC(MUQR+hx1Q_WodRXaFMkAV&O8v`-UJl-cdy zrCsVT$@@OM1jPb(oqW(v0Kgw760;JY1&H$ys1yFOP-~&XDV{XIR zaI}Liw61x1Z?yAD{-u3$<<)p7Vprs3t9MpS*UhYr7k~vGJ$fkv;&oH$Lh*__#Ybk$ zV#_w6g)RU%8gDs*c>f)H!DsjKQ8QD2_`STxDwp8=dax3%8Nl&`v0Qdh2YNBpDIgY{ z&<%-M4GVh_E9Sdm)(HR-!b~<h(9ZA-em34$qRU2dS6+1%wLsgtbLCrwnkz*BzUFErB0N`uCYsJ zH+<+32GTj}TdJMB31`yz$f-wQh8Rwf))M7OS>NbOTxyiEcVtXtMlv+Pu!w57HnDfO zCy*kE6s>9(On4&hh=+5q-NG|t5md>yuc1^i%|yVQbv#5*9+ zF3)xRGW67G=!Zrw-Ns`395I>07o~KlxbdS<%MOh5?_o;eBBo`uvC!<2cc=UIO+&rI zr?}qD<4ud;BGi!j6E~M6v40ckZ&E?jl4lJ!dv4Xg8-UVc&r`zw-dJ~UY}2V{4GcGV z!zh_>^X#L12W&njzJ$JmZ$$a5(R|CoCi5%t{>!%ZT`Ggf2hSR&xasvbMq@RrXTmr4 z3Tt=8s&^+Ei;L3%E=Xjm2klOm?>0ae@Z+V3IL-Hl#(cZ>;K(cA=A!ZMtEsKE*?bk*WIQ7RUsQv@>0^`fLbW0jwlMwaDV1WDb!o{BvkCPfQec)|KplYOIB+ z9B@7O<-|D4@QbkQ`Yc9qm#nAx(Q{)V2Eimt|L8ebal1M{v~@dEOb%xmHyMg-jia#fpSB% z0|N!~WP{_(ydvML4c8l9Zi0@bBWS#|JG$*k?p%G-5AwH7<;+yijK^Ca`z>=Ue_M1X zJjtCu=gFmu=3(EzKxjTSTiW-bhr4T|ya0%fOtKIs*ja}27GgVi@uN;|L^|D{cuZ!h9Nup>^LMMH)`pbSumRCW< z{>moYR;SYcVRI}$UbYp%YjG)EMGYP{&}DQLbqVVf}@cgV(YNo0Q8%;8wxdYl}xHj*s*CJ^>BvYEd7-70+a-AaQemu^p9 zVJPS{{`^zVQUd0&`t7$uw}P>{y|bQusyO+-+_M_;j~heFF!ZmccbTofXNLDJkGIQ_ z_HAq4_B8u<9geOJ%XhOYyEa(9yTO9z1zdDGhBJ(~8w{W44bK-oU-W$OHN&L`-fT6I zhImYqREJ}`5yy7sCywsGh+im5g7mti2nF6V;pp`VJj2Yc2GfD$dM5E;lv2f6H8E6Z z0IJYiUo=~f61qf69b&=~Df3eMh46M=>L_jFDNZATyilykokfeIfHJ_6l7z*RMf4ST ztn?&=%2io1m?bDq5L9qI1VLiLZe;tsYI+A53k3B&FbpBdJBYMmm+YL|_J#v%Hc%k&ZHT*x;}Ttl}@Y7O7}(#o~G+w$rMmWnBij zDlX2%pMhm!o))4kqMW2SrEh^>0v@oFoei>fPT*~dFgoGD(4&+Es~HZQp1^@!2zL<< zoWyQ-aW>$tb_M^tR2Z=lrggc+Y`9byT8XcwQ+J9}S}~}j_m_O(19C$qFV$Tt()#R$ z!f0W%2(u|27gJ?~vEF2dG#Of}CNgmUS&=L*gXW^%YSv@;Ph=xUCd;8C4lv=3*e(@54b}6I1$bo^=cbes zIT5$+=-ZUCBH6k-K(-#~5NUN*HtuKl>XzZvEyKG+8Cf!pjCN&F%fnl>o>SLF4$kLE zU&m38uf2$pnyquxVGJIzLF z0p@QeGgV@v{k+I9ksEP?QJM=a*j&y@DY#tL$9WMq<_C7)iTwY+yKj$*<2v)~S3!5v zbT{ueG;f-x1n7;B5Dn-72qE;eEiBW>5|WTyjVy^lvc_>H;3#9scE)JNS)+|7Zf(3H zoMdNkHfIOhij60;XS>wxG$#1Oo6XtT*_l5uQX)Sx$^O1uT~*ypVau89oIP7Yw{P9L z_tved`}pqn_;Iy96=)Q9rK^UAz72aQ%cB!JQH3mku8*@LkP-?sMJEkhS7vVg@AUkTHnW`m} z7%=T<)$RnhcQ<}TX3M*XgQmZLo1U6H1p^ZYs7rC$99l`3d zeFm$Iwsh*dqxvuPc0xP4e?kjGd!#AXblU43GcMn*gYWM2?Rdbq;lWGdhK{dAuHj9C zryTI#4(xjDheBMfN(b!}NL!#rk`Eq(e2_nN(m}}fA;5XjIZVWpg9;G$wLe6JJerPZ z#yGeNOmu6ZjpGif-;HfX=Ao z34>sA$1IZv>S8<`Dq^mKdnW8#4b&1+@4~wYZ3O89MLZ>aUpC^%22IxH2G5nSl-%qY zLK@t0VdI3D0G;CAJ*2|THt=+x5Pwlr;cojfADlLpz1Wqj43N91*+Z-hQKvhzandke zKVg0^t3t?Z{NxUOswZs;+CRXP(F+ovjNeW-{lE4|zoOa|HT8@JuY0^`{QS3Sgp7ty z_#L?V2Y4D9;{PU3^J{vVun)9(vIrl@@?KUI+|7uO!))5v6om=)Et(^UpTIwi7|0bys)5uqrJR9dL>ZWH$QK8b`MR z2?w8VvOkc~;7@IU4HsSCJXIXX-0V-=Ji7J$jQp!j6OVbGN;YTnf#sZ1G@=1N$@~9Xz+MJcGJH!!U=e%5K&M*jGi^#%(11ee?!NxHF^WwPJKUj@DxJSg>paITi zsr87+vs98eOM$*3DoqE$F`KVwhvAkm;6SBaPQlEjV=KH~z>~Ht z?X#vV?Qn{+dcj>;$Q>)`-;yVkV}pG|8;9*!)x?+A8{*xu^_SFghm)hP4Z_*8vrx1~ zc@iy}GxY<3gRnB-?V5clI8ut|;A@LfC zy(VemdiZy!3Grga5KEXZN5kjLa9iE_r!@Pz|gGM5pj5hkl= z2pO@+z^rDR*`h8lsoaQcMJrDNFF1}6fxH=w#J5m>*1uWbV_lVzMvU3Wn!wnnJL$%j zQ#_zGfKh^V%ELP-DP9uS^|D^PnbP#ck?f|5Y0S)Vz=?tRszv^0C=J*aQ8o;Ac<2-N z4k{BJBQrhOcvTOF^DD#SvRl$b_*Xi4Uf#P}$fzB)iY7MMKqGzS)b&&T?A3QP$)@_Tb}#@}%_gM> zlL`g0ekl`@%3yz!m^ohmT=i)4d|Cms)N#P`dC*=U*b63H!J>Mhs6JSWpK%4-9~9aj47MK<+7E#{SoAPyHKZTbj&A*Ba#|qG9e4n`2#@$5=tJC}?8Z!- zkvn0XO#fm2)UK(b*K!5Nrs)JBui2mB8r}Y3M(!;BvWtS*^+I<2bCIu}jEy&E}Rd10qqr>Caj_;ha$<|TVLOOsF zL&i<@-n0OG$XOCY_NjdnwSG%U&{8E>s=V%>TIv>@cu5PD8gPqY=M=^*K6D@4;`!{n z38PT`U?BS;NK2 zCOvojh_@_|RyS&yO-g|tS9%6~zb54^YLZNaa3cqkVYaf(rewUZ|LXqnzULmgoze~S zcWnKa*aZnzddoq%tZW4)`1i?`u(*;B6!_-*;K_MC|X^uVj}3#EAIg>p@4-B0aX z#yyjLQ$621ekJ)z-*Xwa?OUd6r<;8C)=}>LxTI?wbmk`#uC1MDxwdH(zVQlSw-w)_ zg(tm|DsTVw!6}PS;<^$$ZoX|VBt@7O?JrZ(F|~VRZpPn;|E?_nu)CB)qvmK+vj4uW z5z=v6(Zu$N2Ji5X3a5_z=ZdKurU#!pn_D>1bnWQGS!mu*>U}x&Q00kV{h_rmGPAGs zpwIHaUG#~ENPJ*VA5R&5!e`9=WU*gMZ1YbRq0f=ftYu=cw=>P1IWcb+8xa0vt*NuX z@b|h{gb{3S+7!_LJEt*qg7^sGC}N|6bMcTiG0apI6G`-;t{ons&a0x>A=Ub2_2s2< z%iNWSMt0o4!Y}+@$nArORfWB+hJvDC#xO+@6$Z9yDOm%aw2T08%GM+}SR=he5k_56 zSXHy9IY287Tf)Yv1fePL*ANS7uFskJB<_Q_+7%%~heLMq@5cgndT6B-nKce7M zMw>pDSgrpnZcS_iRZhc-ymUzon`r#k`MKgd@t&dxH7C(k{L%GD3utMgwu#pv=5Ftl(wALfm;TO zkT6!~Y`eRf_I38|+u6ONYg|!}}3}Uxp1WSX81}nwM zH`sXFL=S<1{NVU>eXzMlXzuZ?-V-qHWf{mX2bqIWW{Qs&1vATq%yP0IgSu(_So{@t zFx4rfIs?XXmW#$LRc~`F9j^2ZS$`VA6S4k;gy`c^H@*O_M;{NOr>q zpS04~3QLoxUYlTKFBvvxIhnjtf zqHoE@PQG$KDW;49w4hK*Req)GYIv?NAyaKNBo!P?cVieoHWnIHK=^cTm3Rs) z6|;7S6lZXmb8%hmO`W}M-A%i@b~}^!r|9ZGQ!qkf_!kuGqTmTSC5f=8UUQM9QSem? z*qlJh#XQL*n7?qIrxM?x(@!Y)6oF)S;-E|U2>H60PW|DjxJ7&6lT|K@%2ZXMJ zz6TEZcJ}&uANE-elP|VD?T5DPdnwc&YWSxjxKLw`t5$!jmUC^0TsCE;l9#c7hEpYK z)0#9Sz>Zg4Cc}MqAqP=BQ3rB_1~41=@-(7kLk@#dS7tyjYdl-?dxgs6mO zFa`Ufa;0@rObtVjL7rDjIh5Z4q8+1-A!5n0V~B*8>`+`Wf~v+`L5K9qmW9<~T6m~h z_c|%Zkc^BlA_FxqQ@5;8A(d6g&w))!niRv5!b!^q!%`r0gfNso_!vzei5c1~nDQ$>E!t9WHGpT{AjTO@Xtwd2ruTx4usXbC$`ITxN z*r%jPG5Oj7DMk5}t^vwR?XJXuW`|1q3B6LK%!s)cUm;ZLAt{F5FQ-arctiSuL&}*H zlXi6wDxk&eTf_mx-8As9ltcL)=u_fS49e_oDX#nu98uy@ENoPm@4I%(=ht-k0Rs_t zN2PlxziMM_B#anBsR1Ro6jLoF_e3_HPfB@}Uq#W0<@l1LGHiB}ePCl8#f7EI(DX>R zNIhOum}00XRymC+0I@W?oW@j$+=&c%HwoiDS-!$lcHF5@bcs{G&iOQFdMCp~5#{LX zF{=%mfI)DEXLO+rfa{VKm*d!=2r)1m>j?pir86LGChhP@X32!Gy>U1eInE41n3D~) z1xbkDU^&C898QqMJ`U0W_f$HDb3$4UlFBmD9_jZCIl@FipXwctZDevf+=nTBY-Ccn zWB&`p0Kc7{!FuwfLz1wzw{YmdxXcDJ;*H1~)!t8)!c3$F<5GDWf1Dfl4; ze@nqM1q&3sK>;(Q_ESut;IAp5nL%^|n2lTWCn#W8EjQ=_liTdarcME3;lD)}7$>WM zVvI_Dkz%w6^UV}|LIKgd3ec!%fBh&`^))C*SjKs5piACVj`x8sZqDpkl3Fwwg zV0#9O8-(Hp0&*#cxKdgXEZr=WZl)_m3=gJLsP3e*3K=?`&T{8rVr6(G{(8JW3+z1r z$$(TBtyAeNCj?U}@$@q06H{NPzFO^1EI?D1IlW5^6J@>bfIsOVaAf5CkiauA zoRl12O7T_gt99OUH=nriguh}FH1E^$d}*a)L*D(ts?9>x=3tdesB%HPLX>{GU`!XW z!d^~$wSThhyMuxJjR9jLk^bOD293l|eDTbyh2H+}mIv}y1&phscLfs}-c>Lx%bQM{ zNt%wI*@p)DdJg*X9tap86kTqQ4+Tos1WPvwrJLYneR|bQ)0^v=(fhioqN&uWv(sm$ zANA#R`V)8hEIVg2bAy>Hh0K+cW&X?!W42M_mE!kp$;@5%Mxf03ERC}n`2fJP&ZcJJ zCk`^H@yD*EQ_8$Ovs$*R7+RVi4zg`D;NoCYaV z=6rI_b35Qi2?W)bPF*|YJr}HL6KdN0nOhe%I#U%47h0~=%~}$k z*)g_5G(H|b6U?g-h-F(ZG9%-PJm3GKDAi8VrIg}2$4v0_p`*)o;?+SaK9LOnr~)U_jDhc;z4C3Acwq*spu zifCd;NTJ-xD<-W|XQp?(es0DhY}x}ySH8vv(C+w$w0HF+Pb1k$?ozIl38NBYN^^&1 zxaGK~1h0G~HPdu%vSA*RQ+uCA7DFeRv6 z;!Lbe+m~sEjpkg&OB@L%(^f&!3b7>EWuB%y;_ipIm(-gOI<$=V(Be*@zyK@xr02hbG#DluGYzU-HVyq{*JYIrQ40X}FK*obmYT zcavCe5BiGEv{y?2RH42S{=N-p3F|BW4iOTZsnAra@1L|+_;LyQKn-$=&kEx`hTNE` z3g5TU&R;-+836@P=i(JxShzlA_TLSRZ0 z3<-@M7i*TdR9SmTlp*W=sQ3_zxPG3#-d0qAua^K|&@v&RESOLuB-8*^J1*hMk@4=U z1Hsht+o|RAN$KOM6S`}e!Hg;)qsn_&$XMY|TIqw--IgSRg8GaqJY`~turETi{^yKd z$`f_q&C&OaIwBp&N$(llMB?kSaWQ8nkMq4fJA1pjTNu|5Z;i3*$K){(-RY$@o?g$g zYdl?if?gTz_E?7x=dgQ=ZdTu@o=OwSoBXL;!2O!F;Z;uBFfy@U40QRa zVs>EbuMM}eWnQdg*%7RFSCXwbW9)s7#7K>Q571(BMl;vH37L!=B}p+RLvg&0&R8KdTThZCcz;zotvv3gNs zG4*HxH~`XU-E2+?V@1P*Pvf;lQUTc(rUGJ`E{1bMp=4{A9>_NBhXjGlVAXnFIduI{ zz*5hE*2pu^#5Mv&*2Tp4Xc=JJN+E6Kq!Z@9wcGq@+pmb=#$|%N%$po^HVV$hsUg49 zHRJZ#%L4Y@@2BN2BvIq;W-@;~8>Y5THTbf&`mNi1#%&+pjSU$~qVgQa6?3#t9-p&e zYR^>L^tzedGcCT-?x20QVBgLB@bmy^n7Bi4tX#cit?tdWrsi7iEp2IY757#Zhj@e( znC3_}qntw|oSlu)gAWybM9!bfv$e?zk3k!ONgjP*og|@ssCLY~1`@QBc%v1zQ6Jl|LPR}7PW>Pnj_Q=b2BPql4)QyN+iTzhf z;e_jz&g-3%&~97rPuT#?pVZ7?YN?P~>Ma*i>qp~cx_BaPa+S}R7cj1ywWThdvFZ8Q z`+XA4T}~=0zE$=%kt_Nb(K1wh9l0Qd5|2!YhEo*yTC_Gq9nw3-Rhbd5WRb42@1!=* z%2HJ}TicY9QcR+NAzPGZ+6du*lrii#a_*MraH4%xLd?^Mv=nRy2l^LAu5x2GN?aPj`X5E85tzs?>oy#lTVw%wnvI_^OZw)5^%PRnWv% z!JM$i3`G^DH>+0`7$`ZVcQ}x*q)9QX6smyBZ1vE=3#36|O=BFHmqATpZq}dLE|PJO*8tQJJD%10$Jk(gi1FA)$mQ9sLT$E>fT@EXt;W_)X3jkvl`m zykNtSNf12k!S&0jN62vi!x5M>9PfwQK_Xi)X^P!XDQpK9I z6Pz4cn(kpEUVz;Rbg!_{`Kiq{o;+DUW%k!KKWn&cbKSNboH6`2>$_Ipz5~Af2Yg!( zf?<-JP9XFUwldM`mCW0O(SrmahDmtq1R5MAH3ff!HOyDO-nV`SmXPi}GaGz)`vS)O zv$phm3DgcS6$*Z{h`myasD1cmrp>iN|K=vMtHJP=*?{oY3eL4IQa(ywgiUyVg~%U# zLD4Cbo*T=E`m`4(%jnoLL{p1AeV|dHgUHBN4bmit;lmL!9n-`6C9?GJ&$I3a>3?&} zjhN__hy-dP*OaYE2C>I7e&e4}xmbd^?LRyerKo2O5jUZ$DqAeE|AwxUtkFZ<*ZW(O z5_4XT^EPAe3uOJR!kVqV)m#0JZ9?KUaPA~N&P1$th^<%ks_PZ4BJA_Mil_LppHERR z#Vbeo=p%A>dqfuj>%*M<9X7M8e`+oBNgQes&EXKr8jv3p zyn?OZj%D+$itszCL&dV4iM-+|PYI+bF3idtqCGWjGg{Mz7WR6-uefuhd>ueIfIR0jpdb`R)l{XDq~9;qfNW%^l7?_GA*p9W(?DX3JShT!Jkloe)>iGC$l)4Jc%0T zY;3WDCZM$XU8QG;4ei^QXa@2KdISQ|42dS_RKe6?hGF6=xpbvuMVDPE+12mSEOY>k z!7K#WNLMhsO2~#~gpgh5Pp$t!|CB4(&>=K*%(#Vy-Tu`*K6o}Y&u3!ab2Ilw?o^E6 z+~m*PJZcknXKC5vyPi9@sIi*5wB#r@L&Th)7@l_dQcCKab_VS`1^Z66U+cnt zZ3Q~ToAG(B4BeZR>8^P0EyG&GZ^d)2RMV}*Sc+$uT9UY11*I)k?#EUR@d%%1@;>92 znAeu$lu0A*{vD=Z2BQT%>Hun;%5EbC)4y@nqy{GXH03IraA6ckARGmZFELc;&hOx_jk)7@jg;X{8jCoyb4b0DJ$u2IAml!CM|=XIoNr?UwiFFem$DtED>#VY35P1E#}Oy zp5^-=gEP1x(MhWX2`7#X9%oKkMcDbzw-2HLB@#5uQ_ib+Qa*{Oi|urQxWA$@#($*K zf1=ZWrhw>J=Dt{TyPKVP$P>X^R=jn*1$uPmelo;O zIiMXieJH(a1-O^N^_JECG_`%gFzKFl-d?@q*&$zQ`)C^i)|yq5k4_%&xsrb+7%{k+a3n6_0JX0i+j{~zXEi~Iuc$J$$( zcDKNIPuU;*2@RZZ-t~8AIC&9702a(i+yQsLr>}p|y)1Vibp01JbV3|!6>2*0ayc0&r*&x(POAsbLY=7pqHa0G5@3W(TB7yu^p()r&e-F zvIhT2<&jq^ePqs@6A>ayGjI&45t{<0q?@#C}`8H?!DLxD1fOD~jwY9OtyC zo8UC!Nk50CRPzxN?mp_ge#uUIUC7GC(Ahd%$wQ9b~ zi^8QY9BSCIT{v`|^cg)bp~?t>ILTQ8#xj|a%DS$al}NC$OK9v07+|#w1(_XR18Xdc^v^`=d3O3UOeRmD6C`?;&e1?^>1mpX7m5oJWoScQ}+&@ z+!aX9v*h`TzfO}DwwM})lK}VZ%vdqRv+O-{S_O+fI?G-9@cY<@SK}1W+*K?j7kh0& z@*2!tiCHfm_ZE6RlRd#zt-`8SUwx}Tw@pZFTZXflvUFaf`H0PH2B#6TiM)?g@=4d#v>=val4Gx=VP3)oy@XsIHvD>ADC+(nF+=1ME_7fQ(T%exOR57@aL$f znZj&h6+79d(IEac3Y015E}cftJCX{{X_!_otAaxpKckK}iYg*zoNyyo6;oLA)8;FO z$86sq00>e0GytEA(IS;pui>WohIwLSa-A=a>8p~V9eHgVf9!Ug1D3c4uOB3_kxQs^ z-7a>`CS}apvc;Z?S0Ep>+OeKmf<8+<6}?Bks6^!6BPZc2QMsrCJs?7p<%?ElRei=K zbub>dykaW1=h4=sT6sn=vDn*G;pBv_aF~NV?I;~uS)YyvJf1Q0 z8}V$>B%6-$brX9hw)!%v0>dI%N_ypC!iw>n^{S%`@>ZPCudo&nC?Rtk=l zlSP7KjX!hkkBX-a!Ogpb&2VbJSJ=GIzka_j4*#Kv9y{~JJFTd8Gzn59eFIY78 z{EzRh)Y#!CLQ4-F=6->c41Fc8>D|*U@Fx<}zFJ|o?ZcI~J44D?jkn-qTrzY1b$ z@gXF4vSR7x>E`KGzM^eG%XY!Cot569{S00u#3JnZWPcB0k(TMc={>$8n2@v!mUdR8 z14YcNhyf42w2hml^;2hjMJ)kK>rz00{NiAKt&m?kZ%?^$_&MW?#S_}UD0|uUO8fQp zAEZp4{eG^$Xfv2o1zWVEE|{(OEV&=r9G@-3(XBr945IJ85x=G-LGz|Pwbi75%bwMm zqIs(_t1Vu0%h+Vx%4vR_U}}xg|2VUtRjYqn%OU=@iEB-aeLKa}X4b!*ThPYo-{CmK z--$PE)#=|!Eoj@Qe`h0y_zY*-Zr9IPo7QfR)Bjx@hm!~%6G>YbEtVUi#ndP<%%SM^ z(Fam7BBBqZs_YezszOU4m1x1J1F4p%B%C5ug?>V+ye2ATkSa@gB2r~JPDE;|awf%4 z1F4us9Z1cLO2VmI`=a#%fSRDYC{s#m2qY4H!2cE+qWa@yn3cLvW4=m_vOMaGP)+1I z%ZCc3delt*6-|=+u2FlrocWe4 zaDU%jd--3^GdQjxnIZul=VSW)pZ5k-=o6xHpkYq{0jAHVL8SK>`Q(f~UlsL0IA!&k z87P3<2QTOb@p>ZKe3a2mU$V`RixGrDSl?kgztu}y;kJbM2J+qnUtrzReA2T;^g@;I z=Z;stmD_qj$LK6rAPgEoXGK^duq)g}4!*{0xQ3WQmpg5pPV1auTj#bOo(LYcujemN z>?sPqPQf!2e3OD}6ud;iUs7_L51iqbSCcaxQC&GnXHt%7!T*Rc!vN6kMj@SqfgD;9C^@dkX#o1(Otf zpMt-p;B^YJh*&yF0i)xXJv77dIYDWRtoxL1%1AgO@a9ZIJhX8hIot=&kw;GPCl3$7 z*?b@WBdYD|l$EhJeubDsof*j|?umZhGsiKu8Gn)Tw$cq+sK&U^!%{s6Nn54zqhG7t+d=l$`uv&I%!Cg>s2ZXIBWBE1;<=-t?m!Te5l7xZuzvq(QUR zn)J-kv7?s<;5ceL=IJMxDh?sdPNW9xWl%VJ;po+)&ksP@^}_b6+s8f6?*#8KEt^um zIq;!9l@VytAWS-slH>>7zp7MtHm`*fL!_!%d&{CF8Ezgk69N zVvU>!QEI+w942Vas0smi@k_1HQ6@0s8|By4VDI?=N*+>+tUJwoTSf_D$~bm2dE8H(YJ`cs|Xs zsNs+UuVUxbo&MzFU~+|!T;Z#3ocg1wlfKIKKyn9N?0B{VKDm=izDZ}naoJdj)& z_6BFOtG?L+5(}06?l%i2H1$(YGkV~y8cS=P=IuJuc8mV)hIQLx_3x}s-5Rf(v1k#W zi8pO)M^OV^lvE z-`m><`zq{hPn_uO#jE}SUJAny+{{$rc{jZz7SM$@JkG~YY3}M;wOsCEmo9>YO}em+QkMfw?n%aW9A;vE~ZL*LEk3M^MUF%rSF~u1MLQD{xSYk3ay;&SEr-x*K%zy*6)YNU+;;6^ zfq_d~wAi?e#R3j7YZBM2y;GinCthtpqQ%T*FQ#*7iq(jUk{w+BuU*;-u7WXa*_-Xd zo0SjDuHsH%5wAL2b6Rb05VkVQoH8lxf<{qtm($?SW56WeOOA}lygRJ%aZn3UR})iA0Exc$+Gk^`;$S4czg40w0qp#~SG+=oD0hveM~s6;*4 zPhQ@9HU!;TNY>l`CM}Wf2>an4fCuhF>*r$Jhk@2P2vbTT7vS6(FrMO2r0o$rW4PVD zgFunt$>?q_rspVu$-*h->0^WNnn`FRJR9Q)JNnN*a_TVe-Zlt{34iAF5FBP4Ju~D! zb#`#h0xf_c{?vI^6XDQOjXX4mWX)UCg4SHY znhW&!lh;p9xdPUvOKr1;xUY5mamQ0Tv3hYC#MUv6884G>Xj6iSD;kb5iuk+{E`i6a z?-|oSz8j~pewQTR<6rDYS<_92KM;{+wm-@Z(DBY4FTh_`VaT&r1(VJN~&8F{v-) zuNE#NnB!LxBh1;vYf=~V@T|L=#Z=beX~QCAgES7yK6UJDN;Qeu9)sg^+Ai`WKjHIO zesTbNidWj^pj4~p=;71-N@6}GT0BE;C8+>OBH~aHU5p(%QrpX;{$fg}-8f%L>8Xz& z^N@4nVbQw;`?mBQz=6=r0Tc@U9|k?g`g)I?I``-ad_)G_TrGWY(Y~#mYOA1t_O5&t z1=SSL=9FJS!Ac59cXcjrxjG5<86%djqpbB5{F(x$m^Xu|gx^KQziI+Ip+PlMnN1^1F;w7OquD6nd@t3S}>e4r`* zK(qD(&B_lnS&08!(;{eEKG2kWpmF|OQzK|#pPp_RX|yy5JCJZiU_NqfCMm2HZkkE_ks)i)q5`@+a*Ec z46LOiPLql;nPB5o9Bt!I+i^NJ?X;QZ&n{rZE1BA+KgvIf{!sg)eb3#!yNjAkduR5X z_nh;*?>*=JI`7@D^m+|~=XZbqZQm|ELf_(n^r%wAquT^R7Z8nT!jD|Zo^TQ3m~@e1 zFLTKlvQOfWBtYM%%^IXg8ZpF6v9N>ZpYK2#pT{A@sIJ34@ssY!-T}E+D<~Id$Tsb05`EvtimzhAlh?f6?a9N;N zK%Yk|U8`u7%Sx-yAXh$9Kx_KT5n|P91n9Ya4kk(6QW^AWPnsC|PvJDc0=%nmTG)OoMikIZe z6hba{JT%~^8P*;41l@fYB$(54eqRrtn$$A>W1+#+KtIU(INrT^<*<6?uqHWlC{6An#~xXmkoNZgnw2{^dT>KM2^rf86|ts&&Y8l1vA6sSzRwWs~^+#5VZV^YFIX=2TB1H#pk3YDfwK9 zepw9htJ16a5|!z1sIFsO!!s<85`9HUI=kg<(uh_^WwhpcO6vsr4*Z%lUK^#N@(i9| zE3Ndclc-ED=`)J3UWOG}S}o0>BuYZFuK!W0q96Q*ZOoFoUXqpRqXezHq0hu{7W(`4 zkpF+}ff#S}p~UZUut zPts8?%C?DIt6$(+16;{9%vJmxmtC~LcIjMwJ0&X9zaVifNEG>KEOA}7L_U2(fu%H~ z8ZYD)us6KqtcI_bWR7Vdr<9yg#q>=`@;*reEHR!R(}4Yp!2a4{O;pPovUE@y)#AHQ zLZ2qG>{<35Hd&w6{07)>d0LBjG~?56q@=arTN7U0E4~fY>Br zvJ*I-wJ}*#yzBbXH)Do7qnT*hsu{hGLk1Z_*NAq9T#!Y4VSx&I0*s&vhd7^jOob;L zW`eXJ@9~E`yn_;C0j`%_4f0uhDF{l=6A1emj>Frw+wCLIoK=G%wx0`oyi64oDoiiy z!IHQtHpErq87OKvpBMCag7NWHoR{^5d9Es3xs~A|K^;m}I$XpW@c4q2b^ego$WoJEf?~g6N-v74bIG{G>i& z6@lqW8s0B%!XA-T*7QWQSgL*ZF8k)nS{9dqsgo?$M{-5RWPx3s;z`3_t=!Jy@2eH4 zniNVS2@U&R@Hc$bKEgH3hr~fdz=vQI9f1uG6EO((6X@r}1%g218XU5}k_{3pF72*i z8bmKkp>zg*O1tW+TN z-#UDUzI`N7cl4?tF- zv2AyOTDSB>G`p?D8(U&;De>5VQ;IG#R?LP-JK2X zT`kV`Zg=aU#)ekGoD#te982aUI1r1IIEynYs2IFgPzOTbPNAUBE0CdnLCuAIL3b$V zWdym8X966gNY6=Vd_h6i*3$0AJL&3ZXb0Os>H`z8p#jLKCcK8fxtdb@6f|8!90+ip zgu_q)4+I?<_6f#L=WB;sIsw+v4BuK^g3J>f5_B}f@oZ=)d0G{Be1NAzgF%NuP~q4V zb2o)Gk<8Vl_zXA+2ui<)hg?eoj`7fbUl6t}2A-fe;qebJoFEs&i`^=sQ#NMTVF5TQ zIsCl?i9YolweQ)JOhvF`UyywpRcE-S1+D3x)|SxPCf}LWZX9V^D6NcFAB=BqO_a8N zZD^Zp8sjHAV<%?}Z6o{VjX5LDk13ga>thWmU;kKzj0N)s(}Jb!o~1Hjsa!CZ+%wlC z%r%ddluU)$eLD@cbO&8yC-07U7n&&B1YVP%^cW%CUM+tKaM)SFMWADxy zi>JyH#tPV<#(1tgRz67uU=dIHsFt3brh$^yiE*<0Lg*a#vOPScbHw7htmm zTb2|^Zya&X=NEsfzo@@tTt<|9O-wgNjrH8u<}4Jhp6t2w$_?4{zU$gVVa=FxqV(*+ zhlQ(?#F84ok2R>+@wIOM1U>D%S$gf@?Txb=_nqyI>-LX0p#k-cZMswTbwTs3(#g^* z@+oGz>2m+g&9nB}L}BerLGv8df-w~n$LFZx1*K-J_Wj75rF2GFx?sqkH{^e8o;-Z1 za8eg9-SbD=9mAJ&T-WtTMi>thONdY(CLStvasB#)a>I1(jPh0Bi5F~{HPpscwexso zZ9=(j$~&WUh|+Mr{Q|~3# zZo4Im+v{!}jO&^){(y)-kj7uJT(w@d&M3>pm#pMD07}3-Eh0|~@sD+z?-@LkdAH58 zbw}erbkEfty}RS+*@3vhGqN9=`$_{oL{ZgE<@8XzV8^UsXI!-tR5Y(yd!=NG`K)5f z1}Cq~pU+<%U$ZTdzx|duZrue_yDjsUf_NeLSJh2f+`I**YOV8D+oU^T-8Aiuud0Tb znipqs7jpAH2+iiMkLPSy(#qA2$1;VyV#y%7WL|>Go1?5a3ffP#-=!>1mpK9%+li;k zJCVMbcq$Y$KV7yX<1vE6Vf_0VO=~^%r>dMbiuyA_;Wo$Grq=vrLqVH?`U++Yl%R6E zaX7l&5xJPL;p(OE|4ytGBshxMcIY5`aqJEtSX@I1R2XL*1r%ot%L_`{$BLz((sK;b z1FV2ZEWzy-lu*gJncxXQ!34PhmSKN@`PE`BuKW=Q#mO}*>l!)0g;+OKj$9=!A8|$% zbR17-rf_~`cjKl5H!Zkn#SKm<_}zFjZa3S8hid0ZFBBz^oCO(k(#syelM0Z>uq=x& zQL%tYUVf^&K!_gjGrQR!Ea2jtn}KFYMi9ieNc#Y(A0W*GqsT&}pQ+H~KLKbiBBz?3 Date: Mon, 28 Sep 2026 23:04:42 +0000 Subject: [PATCH 4/5] Pin ROS apt snapshot, clean shutdown and restart health, fail retreat into recovery Co-authored-by: Mateusz Sadowski --- integrations/ros2/intrinsic_moveit/Dockerfile | 16 ++++++-- integrations/ros2/intrinsic_moveit/README.md | 12 ++++-- .../ros2/intrinsic_moveit/compose.yaml | 1 + .../intrinsic_moveit_grasp_demo.json | 16 +++----- .../intrinsic_moveit_grasp_demo_playback.json | 16 +++----- .../config/demo_params.yaml | 2 +- .../grasp_demo_driver.py | 40 ++++++++++++++----- .../intrinsic_moveit/scripts/entrypoint.sh | 9 +++-- 8 files changed, 70 insertions(+), 42 deletions(-) diff --git a/integrations/ros2/intrinsic_moveit/Dockerfile b/integrations/ros2/intrinsic_moveit/Dockerfile index 1e0c08a..02628fc 100644 --- a/integrations/ros2/intrinsic_moveit/Dockerfile +++ b/integrations/ros2/intrinsic_moveit/Dockerfile @@ -1,11 +1,10 @@ # syntax=docker/dockerfile:1 ARG ROS_DISTRO=jazzy -# Pin the base image that this tutorial was tested against. Jazzy apt snapshots -# on snapshots.ros.org do not include the September 2026 package set, so the -# ROS packages themselves still resolve from packages.ros.org. +# Pin the base image that this tutorial was tested against. ARG ROS_BASE_DIGEST=sha256:c3706ef0a0aa45413c07803cf433602f543b22e45b4855f6fca955c2d8ecc4e8 FROM ros:${ROS_DISTRO}-ros-base@${ROS_BASE_DIGEST} ARG ROS_DISTRO +ARG ROS_APT_SNAPSHOT=2026-09-11 ARG INTRINSIC_MOVEIT_REPO=https://github.com/intrinsic-ai/intrinsic-moveit.git ARG INTRINSIC_MOVEIT_COMMIT=c5e3290aa0f0e64c2d106a2fb4eb10cb52592205 ENV DEBIAN_FRONTEND=noninteractive \ @@ -14,9 +13,18 @@ ENV DEBIAN_FRONTEND=noninteractive \ INTRINSIC_MOVEIT_COMMIT=${INTRINSIC_MOVEIT_COMMIT} \ ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST +# ROS packages come from the Jazzy snapshot. The Ubuntu archive is left unpinned. +# The snapshot key (ROS Snapshot builder) expires 2027-06-01. # ur-description Depends on rviz2. Extract the share files so the URDF resolves # without installing RViz, Qt, or LLVM. -RUN apt-get update && apt-get install -y --no-install-recommends \ +RUN curl -fsSL "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0x4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA" \ + | gpg --dearmor -o /usr/share/keyrings/ros-snapshots.gpg \ + && gpg --show-keys /usr/share/keyrings/ros-snapshots.gpg \ + | grep -q 4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA \ + && rm -f /etc/apt/sources.list.d/ros2.sources /etc/apt/sources.list.d/ros2.list \ + && echo "deb [signed-by=/usr/share/keyrings/ros-snapshots.gpg] http://snapshots.ros.org/${ROS_DISTRO}/${ROS_APT_SNAPSHOT}/ubuntu noble main" \ + > /etc/apt/sources.list.d/ros2-snapshot.list \ + && apt-get update && apt-get install -y --no-install-recommends \ git \ build-essential \ python3-numpy \ diff --git a/integrations/ros2/intrinsic_moveit/README.md b/integrations/ros2/intrinsic_moveit/README.md index 1130fe6..354b40b 100644 --- a/integrations/ros2/intrinsic_moveit/README.md +++ b/integrations/ros2/intrinsic_moveit/README.md @@ -61,7 +61,13 @@ flowchart LR No display server or GPU is required. The container launches MoveIt with `headless:=true`. -The base image is pinned to `ros:jazzy-ros-base@sha256:c3706ef0a0aa45413c07803cf433602f543b22e45b4855f6fca955c2d8ecc4e8`. Apt packages are not pinned: `snapshots.ros.org` has Jazzy indexes, but none for the September 2026 set this tutorial was tested with (the newest indexed dates are from 2024), so the build follows `packages.ros.org`. Versions observed in the image: +The base image is pinned to `ros:jazzy-ros-base@sha256:c3706ef0a0aa45413c07803cf433602f543b22e45b4855f6fca955c2d8ecc4e8`. ROS apt packages are pinned to the Jazzy snapshot `2026-09-11` on `snapshots.ros.org` (build arg `ROS_APT_SNAPSHOT`), signed by the ROS snapshot key `4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA`, which expires 2027-06-01. Override the date with: + +```bash +docker compose build --build-arg ROS_APT_SNAPSHOT=YYYY-MM-DD +``` + +The Ubuntu archive itself is not pinned. Versions from that snapshot: | Package | Version observed in this image | | --- | --- | @@ -126,7 +132,7 @@ The arm stands on a grey table. The shaded rectangle is the OMTS return-shift wi | `RETREAT_UP` | The tool backs off | | `PARK` | The gripper closes and the arm returns to the work-facing home pose. The cycle counter increments | -Home is the SRDF `ready` pose with `shoulder_pan_joint` rotated by π (`-2.8173` instead of `-0.1597`), so `hande_tcp` sits above the OMTS window and points down. Joint transits (home, pre-grasp, and place) request Pilz PTP and fall back to OMPL if that pipeline rejects the goal. +Home is the SRDF `ready` pose with `shoulder_pan_joint` set to `-2.8173` instead of ready's `-0.1597`. That is a 2.658 rad (152°) turn, not π: enough to face the window, then back about 28° so `hande_tcp` is centred above it and points down. Joint transits (home, pre-grasp, and place) request Pilz PTP and fall back to OMPL if that pipeline rejects the goal. The loop then plans a new grasp for the billet at its new pose. Feasible IK variants are ranked by weighted joint distance from the current arm, after wrapping each joint onto the equivalent angle closest to where it is now. `grasp_quality` only breaks ties. Variants that would flip `wrist_2` by more than π/2, or swing the base more than π/2 away from home, are dropped. Up to three distinct poses are tried if a pre-grasp motion fails. If none of Intrinsic's IK solutions pass, the driver calls `/compute_ik` on the pre-grasp pose seeded with the current joints. @@ -217,7 +223,7 @@ The standalone node only compiles the SDK-free sources. A commit that changes th - **Port 8765 is in use.** Stop the other process or change the host mapping in `compose.yaml`. - **The robot has no meshes.** Wait for the first asset fetch (tens of megabytes). Confirm the bridge is the Foxglove WebSocket endpoint, not a raw rosbridge URL. The subprotocol is `foxglove.sdk.v1`. -- **Offline playback has no meshes.** Toggle the URDF layer to `/robot_description_web`. +- **Offline playback has no meshes.** Import [`foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json`](foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json). It enables the `/robot_description_web` URDF layer and hides the live `/robot_description` layer. - **The arm pauses in `RECOVER`.** A sampled place pose was unreachable. The driver detaches, returns to the work-facing home pose, and samples a new billet pose. `/demo/failures` counts these events. - **Logs.** `docker compose logs -f`. diff --git a/integrations/ros2/intrinsic_moveit/compose.yaml b/integrations/ros2/intrinsic_moveit/compose.yaml index c4b6659..e4d934e 100644 --- a/integrations/ros2/intrinsic_moveit/compose.yaml +++ b/integrations/ros2/intrinsic_moveit/compose.yaml @@ -4,6 +4,7 @@ services: context: . args: INTRINSIC_MOVEIT_COMMIT: c5e3290aa0f0e64c2d106a2fb4eb10cb52592205 + ROS_APT_SNAPSHOT: "2026-09-11" image: foxglove-tutorials/intrinsic-moveit-demo:latest ports: - "8765:8765" diff --git a/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo.json b/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo.json index 23f10dc..7b71008 100644 --- a/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo.json +++ b/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo.json @@ -507,25 +507,19 @@ "foxglove_bridge": { "visible": false }, - "plugins.simple_controller_manager": { + "move_group.moveit.moveit.plugins.simple_controller_manager": { "visible": false }, - "move_group.plugins.simple_controller_manager": { + "move_group.moveit.moveit.ros.trajectory_execution_manager": { "visible": false }, - "trajectory_execution_manager": { + "move_group.moveit.moveit.ros.move_group.clear_octomap_service": { "visible": false }, - "move_group.trajectory_execution_manager": { + "move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability": { "visible": false }, - "move_group.clear_octomap_service": { - "visible": false - }, - "cartesian_path_service_capability": { - "visible": false - }, - "move_group.cartesian_path_service_capability": { + "ur_manipulator_controller": { "visible": false } }, diff --git a/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json b/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json index 5e85c9f..4a5a25b 100644 --- a/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json +++ b/integrations/ros2/intrinsic_moveit/foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json @@ -507,25 +507,19 @@ "foxglove_bridge": { "visible": false }, - "plugins.simple_controller_manager": { + "move_group.moveit.moveit.plugins.simple_controller_manager": { "visible": false }, - "move_group.plugins.simple_controller_manager": { + "move_group.moveit.moveit.ros.trajectory_execution_manager": { "visible": false }, - "trajectory_execution_manager": { + "move_group.moveit.moveit.ros.move_group.clear_octomap_service": { "visible": false }, - "move_group.trajectory_execution_manager": { + "move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability": { "visible": false }, - "move_group.clear_octomap_service": { - "visible": false - }, - "cartesian_path_service_capability": { - "visible": false - }, - "move_group.cartesian_path_service_capability": { + "ur_manipulator_controller": { "visible": false } }, diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml index 5291283..92f61b1 100644 --- a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/config/demo_params.yaml @@ -20,7 +20,7 @@ grasp_demo_driver: retract_dist_m: 0.1 surfaces: [0, 1, 2, 3, 4, 5] num_rotations: 4 - # SRDF ready, rotated pi about the base so hande_tcp faces the return-shift window. + # SRDF ready with shoulder_pan set so hande_tcp is centred above the return-shift window. home_joints: shoulder_pan_joint: -2.8173 shoulder_lift_joint: -1.3542 diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py index bf0f1e3..92afc48 100644 --- a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py @@ -6,7 +6,7 @@ import numpy as np import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import GripperCommand +from control_msgs.action import FollowJointTrajectory, GripperCommand from geometry_msgs.msg import PoseArray, PoseStamped, TransformStamped from moveit_msgs.action import ExecuteTrajectory from moveit_msgs.msg import ( @@ -180,6 +180,11 @@ def __init__(self): self, ExecuteTrajectory, '/execute_trajectory', callback_group=self._cb) self.gripper = ActionClient( self, GripperCommand, '/hand_controller/gripper_cmd', callback_group=self._cb) + # move_group's /execute_trajectory server is up before this controller is. + self.arm_follow = ActionClient( + self, FollowJointTrajectory, + '/ur_manipulator_controller/follow_joint_trajectory', + callback_group=self._cb) self._publish_counters() self._publish_scene() @@ -400,6 +405,7 @@ def _wait_interfaces(self, timeout=180.0): actions = [ (self.execute, '/execute_trajectory'), (self.gripper, '/hand_controller/gripper_cmd'), + (self.arm_follow, '/ur_manipulator_controller/follow_joint_trajectory'), ] deadline = time.monotonic() + timeout next_log = 0.0 @@ -642,13 +648,11 @@ def _cartesian_to(self, transform, avoid): def _execute_cartesian(self, transform, avoid): response = self._cartesian_to(transform, avoid) - if (response.fraction < 0.95 or not response.solution.joint_trajectory.points) and avoid: - self.get_logger().warn('cartesian fraction low, retrying with collisions ignored') - response = self._cartesian_to(transform, False) if response.fraction >= 0.95 and response.solution.joint_trajectory.points: self._execute_trajectory(response.solution) return - raise RuntimeError(f'cartesian path fraction {response.fraction:.3f}') + raise RuntimeError( + f'cartesian path fraction {response.fraction:.3f} avoid={avoid}') def _fk_pose(self, joint_map): request = GetPositionFK.Request() @@ -1111,16 +1115,34 @@ def main(): node = GraspDemoDriver() executor = MultiThreadedExecutor() executor.add_node(node) - spinner = threading.Thread(target=executor.spin, daemon=True) + + def _spin(): + # SIGINT shuts the context down under this thread. That raises RCLError + # from the wait set; the process should still exit quietly. + try: + executor.spin() + except (KeyboardInterrupt, Exception): + pass + + spinner = threading.Thread(target=_spin, daemon=True) spinner.start() try: node.run() except KeyboardInterrupt: pass finally: - executor.shutdown() - node.destroy_node() - rclpy.shutdown() + # SIGINT already shuts the rcl context down. A second shutdown raises + # RCLError, and a second interrupt raises KeyboardInterrupt. + for cleanup in (executor.shutdown, node.destroy_node): + try: + cleanup() + except (KeyboardInterrupt, Exception): + pass + try: + if rclpy.ok(): + rclpy.shutdown() + except (KeyboardInterrupt, Exception): + pass if __name__ == '__main__': diff --git a/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh index ecee279..1f57d9f 100755 --- a/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh +++ b/integrations/ros2/intrinsic_moveit/scripts/entrypoint.sh @@ -1,6 +1,9 @@ #!/usr/bin/env bash set -eo pipefail +# A restart keeps the container filesystem. Drop the previous ready file +# before sourcing ROS so health cannot pass during that delay. +rm -f /tmp/intrinsic_demo_ready source /opt/ros/jazzy/setup.bash source /ws/install/setup.bash mkdir -p /recordings @@ -10,9 +13,9 @@ if [[ "${1:-}" == "ros2" && "${2:-}" == "launch" && "${3:-}" == "intrinsic_foxgl child=0 forward() { if [[ "${child}" -ne 0 ]]; then - # The launch process is a session leader. Signal the whole group so - # rosbag2 sees SIGINT and writes metadata.yaml plus the MCAP summary. - kill -INT -- "-${child}" 2>/dev/null || kill -INT "${child}" 2>/dev/null || true + # Signal only the launch process. It forwards one SIGINT to its children. + # A group signal would deliver a second SIGINT and make rosbag2 exit 2. + kill -INT "${child}" 2>/dev/null || true fi } trap forward INT TERM From 5fad13eb7c2be812083974b09e54d154592e3f1a Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 28 Sep 2026 23:17:42 +0000 Subject: [PATCH 5/5] Log unexpected executor errors; document upstream move_group shutdown crash Co-authored-by: Mateusz Sadowski --- integrations/ros2/intrinsic_moveit/README.md | 1 + .../intrinsic_foxglove_demo/grasp_demo_driver.py | 9 ++++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/integrations/ros2/intrinsic_moveit/README.md b/integrations/ros2/intrinsic_moveit/README.md index 354b40b..9ff1c05 100644 --- a/integrations/ros2/intrinsic_moveit/README.md +++ b/integrations/ros2/intrinsic_moveit/README.md @@ -225,6 +225,7 @@ The standalone node only compiles the SDK-free sources. A commit that changes th - **The robot has no meshes.** Wait for the first asset fetch (tens of megabytes). Confirm the bridge is the Foxglove WebSocket endpoint, not a raw rosbridge URL. The subprotocol is `foxglove.sdk.v1`. - **Offline playback has no meshes.** Import [`foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json`](foxglove_layouts/intrinsic_moveit_grasp_demo_playback.json). It enables the `/robot_description_web` URDF layer and hides the live `/robot_description` layer. - **The arm pauses in `RECOVER`.** A sampled place pose was unreachable. The driver detaches, returns to the work-facing home pose, and samples a new billet pose. `/demo/failures` counts these events. +- **`move_group` prints a segfault on Ctrl-C.** Stopping the stack can show a stack trace in `rclcpp::Executor::~Executor()` and `process has died … exit code -11`. That is an upstream MoveIt shutdown crash; it also happens with Intrinsic's stock `service.launch.py` alone. The MCAP is already finalized when it appears. - **Logs.** `docker compose logs -f`. ## License diff --git a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py index 92afc48..9f92407 100644 --- a/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py +++ b/integrations/ros2/intrinsic_moveit/ros_ws/src/intrinsic_foxglove_demo/intrinsic_foxglove_demo/grasp_demo_driver.py @@ -1117,12 +1117,15 @@ def main(): executor.add_node(node) def _spin(): - # SIGINT shuts the context down under this thread. That raises RCLError - # from the wait set; the process should still exit quietly. + # SIGINT shuts the context down under this thread and raises from the + # wait set. Log only while the context is still alive. try: executor.spin() - except (KeyboardInterrupt, Exception): + except KeyboardInterrupt: pass + except Exception as exc: + if rclpy.ok(): + node.get_logger().error(f'executor stopped: {exc!r}') spinner = threading.Thread(target=_spin, daemon=True) spinner.start()