Merge branch 'main' into improve_lab_sim_examples

PickNikRobotics · Jan 6, 2025 · 07fd3b3 · 07fd3b3
2 parents 651cc2c + 9b7a5f9
commit 07fd3b3
Show file tree

Hide file tree

Showing 36 changed files with 19,906 additions and 36,439 deletions.
diff --git a/Dockerfile b/Dockerfile
@@ -10,13 +10,14 @@ ARG MOVEIT_STUDIO_BASE_IMAGE
 ARG USERNAME=studio-user
 ARG USER_UID=1000
 ARG USER_GID=1000
+ARG BASE=base
 
 ##################################################
 # Starting from the specified MoveIt Pro release #
 ##################################################
 # The image tag is specified in the argument itself.
 # hadolint ignore=DL3006
-FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base
+FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base-base
 
 # Create a non-root user
 ARG USERNAME
@@ -49,18 +50,20 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
       /home/${USERNAME}/.ros && \
     chown -R $USER_UID:$USER_GID /home/${USERNAME} /opt/overlay_ws/
 
-# IMPORTANT: Optionally install Nvidia drivers for improved simulator performance with Nvidia GPUs.
-# To do this you must
-# 1. Uncomment the ENV and RUN entries below
-# 2. Replace the 'nvidia-driver-555' apt package with the Nvidia driver version on your host, e.g. nvidia-driver-535, nvidia-driver-555. Use nvidia-smi on your host to determine the driver version.
-# After rebuilding via `moveit_pro build` verify the drivers are active in your container by running `nvidia_smi` inside of `moveit_pro shell`.
-# ENV DEBIAN_FRONTEND=noninteractive
-# RUN apt update && apt install -y software-properties-common
-# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-#     --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
-#    add-apt-repository ppa:graphics-drivers/ppa && \
-#    apt update && apt upgrade -y && apt install -y nvidia-driver-555
+FROM base-base AS base-nvidia
+ARG NVIDIA_DRIVER_PACKAGE
+ENV DEBIAN_FRONTEND=noninteractive
+# hadolint ignore=DL3008
+RUN apt-get update && apt-get install -y --no-install-recommends software-properties-common
+# hadolint ignore=DL3008
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+    --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
+   add-apt-repository ppa:graphics-drivers/ppa && \
+   apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends "${NVIDIA_DRIVER_PACKAGE}"
 
+ARG BASE
+# hadolint ignore=DL3006
+FROM base-${BASE} AS base
 # Install additional dependencies
 # You can also add any necessary apt-get install, pip install, etc. commands at this point.
 # NOTE: The /opt/overlay_ws folder contains MoveIt Pro binary packages and the source file.

diff --git a/docker-compose.yaml b/docker-compose.yaml
@@ -0,0 +1,30 @@
+# Docker Compose file that will be merged with /opt/moveit_pro/docker-compose.yaml.
+# For more details on how merging works, see https://docs.docker.com/compose/how-tos/multiple-compose-files/merge/#merging-rules.
+
+# Services that are listed under `/opt/moveit_pro/docker-compose.yaml` are mirrored here for merging.
+# Feel free to remove services here that are unmodified.
+services:
+  # The base image that all MoveIt Pro services extend off of. Builds the user workspace.
+  base:
+    build:
+      # List any arguments for building the user workspace here.
+      args:
+        # IMPORTANT: Optionally install Nvidia drivers for improved simulator performance with Nvidia GPUs.
+        # To do this you must
+        # 1. Uncomment the BASE and NVIDIA_DRIVER_PACKAGE build args below
+        # 2. Replace the 'nvidia-driver-555' apt package with the Nvidia driver version on your host, e.g. nvidia-driver-535, nvidia-driver-555. Use nvidia-smi on your host to determine the driver version.
+        # After rebuilding via `moveit_pro build` verify the drivers are active in your container by running `nvidia_smi` inside of `moveit_pro shell`.
+        # - BASE=nvidia
+        # - NVIDIA_DRIVER_PACKAGE=nvidia-driver-555
+
+  # Starts the MoveIt Pro Agent and the Bridge between the Agent and the Web UI.
+  agent_bridge:
+
+  # Starts the robot drivers.
+  drivers:
+
+  # Starts the web UI frontend.
+  web_ui:
+
+  # Developer specific configuration when running `moveit_pro dev`.
+  dev:
diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -5,12 +5,15 @@
     ID="Add Waypoints to MTC Task"
     _subtreeOnly="true"
     _description=""
+    joint_group_name="manipulator"
+    planner_interface="moveit_default"
+    mtc_task="{mtc_task}"
   >
     <Control ID="Sequence">
       <Action
         ID="RetrieveWaypoint"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Pick Cube"
+        waypoint_name="Look at Table"
         joint_group_name="{joint_group_name}"
       />
       <Action
@@ -24,7 +27,7 @@
       <Action
         ID="RetrieveWaypoint"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Place Cube"
+        waypoint_name="Workspace Right"
         joint_group_name="{joint_group_name}"
       />
       <Action
@@ -40,10 +43,11 @@
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
       <input_port name="joint_group_name" default="manipulator" />
-      <inout_port name="mtc_task" default="{mtc_task}" />
       <input_port name="planner_interface" default="moveit_default" />
+      <inout_port name="mtc_task" default="{mtc_task}" />
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>

diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
   <BehaviorTree
@@ -21,11 +22,13 @@
         ID="GetCameraInfo"
         topic_name="/wrist_camera/camera_info"
         message_out="{camera_info}"
+        timeout_sec="5.000000"
       />
       <Action
         ID="GetImage"
         topic_name="/wrist_camera/color"
         message_out="{image}"
+        timeout_sec="5.000000"
       />
       <SubTree
         ID="Get AprilTag Pose from Image"
@@ -47,7 +50,11 @@
         target_pose="{view_pose}"
         move_to_pose_solution="{move_to_pose_solution}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
       <SubTree
         ID="Get AprilTag Pose from Image"
         image="{image}"
@@ -66,26 +73,36 @@
       <Action
         ID="LoadObjectiveParameters"
         config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
       />
       <Action
         ID="InitializeMTCTask"
         task_id="pick_object"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
         task="{pick_object_task}"
       />
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
       <Action
         ID="SetupMTCPickObject"
         grasp_pose="{offset_pose}"
         task="{pick_object_task}"
         parameters="{parameters}"
+        monitored_stage="current state"
       />
       <Action
         ID="PlanMTCTask"
         solution="{pick_object_solution}"
         task="{pick_object_task}"
       />
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>

diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
@@ -18,17 +18,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0.75;0.5;0.9"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -40,28 +29,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0.25;0.5;0.7"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0;0.5;0.6"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -73,28 +40,6 @@
         vector="{target_poses}"
         input="{stamped_pose}"
       />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.5;0.5;0.8"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.75;0.5;0.9"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"