From 279db93bf340787988db90e40ca576c79a5bb684 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Tue, 3 Dec 2024 14:32:10 +0100
Subject: [PATCH] Setup the fanuc_sim config with a tool-changing example

---
 src/fanuc_sim/config/config.yaml              |  3 +
 .../control/picknik_fanuc.ros2_control.yaml   | 23 +++++++
 .../config/moveit/picknik_fanuc.srdf          |  1 +
 .../fanuc_lrmate200id.ros2_control.xacro      |  2 +
 src/fanuc_sim/description/picknik_fanuc.xacro | 55 ++++++++++++++-
 src/fanuc_sim/description/scene.xml           | 53 +++++++++++++--
 src/fanuc_sim/description/tool.urdf           | 42 ++++++++++++
 src/fanuc_sim/description/tool.xml            | 44 ++++++++++++
 src/fanuc_sim/objectives/activate_vacuum.xml  | 19 ++++++
 src/fanuc_sim/objectives/close_gripper.xml    |  7 --
 .../objectives/deactivate_vacuum.xml          | 20 ++++++
 src/fanuc_sim/objectives/open_gripper.xml     |  8 ---
 .../objectives/pick_up_block_with_tool.xml    | 48 +++++++++++++
 src/fanuc_sim/objectives/pick_up_tool.xml     | 68 +++++++++++++++++++
 .../objectives/place_tool_in_tool_holder.xml  | 50 ++++++++++++++
 .../objectives/request_teleoperation.xml      |  4 +-
 src/fanuc_sim/objectives/retract.xml          | 33 +++++++++
 .../objectives/setup_planning_scene.xml       | 27 ++++++++
 .../objectives/tool_attachment_example.xml    | 20 ++++++
 src/picknik_accessories                       |  2 +-
 20 files changed, 503 insertions(+), 26 deletions(-)
 create mode 100644 src/fanuc_sim/description/tool.urdf
 create mode 100644 src/fanuc_sim/description/tool.xml
 create mode 100644 src/fanuc_sim/objectives/activate_vacuum.xml
 delete mode 100644 src/fanuc_sim/objectives/close_gripper.xml
 create mode 100644 src/fanuc_sim/objectives/deactivate_vacuum.xml
 delete mode 100644 src/fanuc_sim/objectives/open_gripper.xml
 create mode 100644 src/fanuc_sim/objectives/pick_up_block_with_tool.xml
 create mode 100644 src/fanuc_sim/objectives/pick_up_tool.xml
 create mode 100644 src/fanuc_sim/objectives/place_tool_in_tool_holder.xml
 create mode 100644 src/fanuc_sim/objectives/retract.xml
 create mode 100644 src/fanuc_sim/objectives/setup_planning_scene.xml
 create mode 100644 src/fanuc_sim/objectives/tool_attachment_example.xml

diff --git a/src/fanuc_sim/config/config.yaml b/src/fanuc_sim/config/config.yaml
index be3aff4..6b623c6 100644
--- a/src/fanuc_sim/config/config.yaml
+++ b/src/fanuc_sim/config/config.yaml
@@ -43,6 +43,7 @@ hardware:
     urdf_params:
         # Use "mock", "mujoco", or "real"
       - hardware_interface: "mujoco"
+      - mujoco_viewer: "false"
 
 # Sets ROS global params for launch.
 # [Optional]
@@ -112,6 +113,8 @@ ros2_control:
     - "joint_trajectory_controller"
     - "servo_controller"
     - "joint_state_broadcaster"
+    - "tool_attach_controller"
+    - "suction_cup_controller"
     # - "servo_controller"
   # Load but do not start these controllers so they can be activated later if needed.
   # [Optional, default=[]]
diff --git a/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml b/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
index 68aedf8..c8111bd 100644
--- a/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
+++ b/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
@@ -7,6 +7,29 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
+    # A controller to enable/disable the tool attachment interface at the robot flange.
+    # In sim, this is modeled as a weld constraint in Mujoco.
+    # In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange.
+    tool_attach_controller:
+      type: position_controllers/GripperActionController
+    # A controller to enable/disable the suction cup.
+    # In sim, this is modeled as a weld constraint in Mujoco.
+    # In a real robot, this controller would activate / deactivate a suction cup mechanism.
+    suction_cup_controller:
+      type: position_controllers/GripperActionController
+
+
+tool_attach_controller:
+  ros__parameters:
+    default: true
+    joint: suction_cup_tool_interface
+    allow_stalling: true
+
+suction_cup_controller:
+  ros__parameters:
+    default: true
+    joint: suction_cup_tool_tip
+    allow_stalling: true
 
 joint_state_broadcaster:
   ros__parameters:
diff --git a/src/fanuc_sim/config/moveit/picknik_fanuc.srdf b/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
index 3fb1940..81eb162 100644
--- a/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
+++ b/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
@@ -19,6 +19,7 @@
     <end_effector name="fanuc_ee" parent_link="tool0" group="gripper"/>    
 
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="arm_pedestal" link2="base_link" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link_2" reason="Never"/>
     <disable_collisions link1="base_link" link2="link_3" reason="Never"/>
diff --git a/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro b/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
index e926a49..a0788da 100644
--- a/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
+++ b/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
@@ -5,6 +5,7 @@
     params="name initial_positions_file hardware_interface mujoco_model"
   >
     <xacro:arg name="mujoco_model" default="description/scene.xml" />
+    <xacro:arg name="mujoco_viewer" default="false" />
     <xacro:arg name="hardware_interface" default="mock" />
     <xacro:property
       name="initial_positions"
@@ -26,6 +27,7 @@
           <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
           <param name="mujoco_model">$(arg mujoco_model)</param>
           <param name="mujoco_model_package">fanuc_sim</param>
+          <param name="mujoco_viewer">$(arg mujoco_viewer)</param>
           <param name="render_publish_rate">10</param>
           <param name="tf_publish_rate">60</param>
           <param name="lidar_publish_rate">10</param>
diff --git a/src/fanuc_sim/description/picknik_fanuc.xacro b/src/fanuc_sim/description/picknik_fanuc.xacro
index 885c3fe..7e8a416 100644
--- a/src/fanuc_sim/description/picknik_fanuc.xacro
+++ b/src/fanuc_sim/description/picknik_fanuc.xacro
@@ -13,12 +13,61 @@
     filename="$(find fanuc_sim)/description/fanuc_lrmate200id.ros2_control.xacro"
   />
 
-  <link name="world" />
-  <joint name="world_joint" type="fixed">
+  <link name="world"/>
+  <link name="arm_pedestal">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <box size="0.4 0.4 0.3" />
+      </geometry>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.23 0.03 0.1" />
+      <geometry>
+        <box size="0.12 0.02 0.02" />
+      </geometry>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.23 -0.03 0.1" />
+      <geometry>
+        <box size="0.12 0.02 0.02" />
+      </geometry>
+    </visual>
+  </link>
+  <joint name="scene_joint" type="fixed">
     <parent link="world" />
+    <child link="arm_pedestal" />
+    <origin rpy="0 0 0" xyz="0 0.3 0.15" />
+  </joint>
+  <joint name="base_joint" type="fixed">
+    <parent link="arm_pedestal" />
     <child link="base_link" />
-    <origin rpy="0 0 0" xyz="0 0 0" />
+    <origin rpy="0 0 0" xyz="0 0 0.15" />
   </joint>
+
+  <link name="table">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <box size="1 1 0.05" />
+      </geometry>
+      <material name="brown">
+        <color rgba="0.3 0.15 0.05 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <box size="1 1 0.05" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="table_joint" type="fixed">
+    <parent link="world" />
+    <child link="table" />
+    <origin rpy="0 0 0" xyz="0 1 0.45" />
+  </joint>
+
   <xacro:fanuc_lrmate200id prefix="" />
   <xacro:fanuc_lrmate200id_ros2_control
     name="fanuc_robot"
diff --git a/src/fanuc_sim/description/scene.xml b/src/fanuc_sim/description/scene.xml
index d55cd79..016abc5 100644
--- a/src/fanuc_sim/description/scene.xml
+++ b/src/fanuc_sim/description/scene.xml
@@ -3,6 +3,7 @@
   <compiler angle="radian" autolimits="true" />
   <option integrator="RK4" timestep="0.005" />
   <include file="lrmate200id/lrmate200id_globals.xml" />
+  <include file="tool.xml" />
 
   <asset>
     <!-- Define textures and materials -->
@@ -82,6 +83,7 @@
     <!-- Define the block with AprilTags on each face -->
     <body name="block1" pos="0.2 0.75 0.575">
       <freejoint name="block1" />
+      <site name="block1" pos="0 0 0" />
       <geom
         type="box"
         size="0.025 0.025 0.025"
@@ -91,11 +93,13 @@
     </body>
     <body name="block2" pos="0.0 0.75 0.575">
       <freejoint name="block2" />
+      <site name="block2" pos="0 0 0" />
       <geom class="visual" mesh="cube" pos="0 0 -0.03" />
       <geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0" />
     </body>
     <body name="block3" pos="-0.2 0.75 0.575">
       <freejoint name="block3" />
+      <site name="block3" pos="0 0 0" />
       <geom
         type="box"
         size="0.025 0.025 0.025"
@@ -106,19 +110,58 @@
     <!-- Add a scene camera -->
     <site
       name="scene_camera_optical_frame"
-      pos="0.0 1.6 1.6"
-      euler="2.2415 0 3.1415"
+      pos="0.0 1.0 1.6"
+      euler="2.6415 0 3.1415"
     />
     <camera
       name="scene_camera"
-      pos="0.0 1.6 1.6"
+      pos="0.0 1.0 1.6"
       fovy="58"
       mode="fixed"
       resolution="640 480"
-      euler="-0.9 0 3.1415"
+      euler="-0.5 0 3.1415"
     />
-    <body name="arm_mount">
+    <body name="arm_pedestal" pos="0.0 0.3 0.15">
+      <geom type="box" size="0.2 0.2 0.15" />
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1" />
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1" />
+      <site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0" />
+    </body>
+    <body name="arm_mount" pos="0.0 0.3 0.3">
       <include file="lrmate200id/lrmate200id.xml" />
     </body>
   </worldbody>
+
+  <!-- Define weld constraints between the robot flange (tool0) and the gripper, and between the suction cup (tool_tip)
+       and the blocks in the scene -->
+  <equality>
+    <weld
+      name="tool_attachment"
+      body1="tool0"
+      body2="gripper_base"
+      active="false"
+      torquescale="1"
+    />
+    <weld
+      name="suction_cup_block1"
+      body1="tool_tip"
+      body2="block1"
+      active="false"
+      torquescale="1"
+    />
+    <weld
+      name="suction_cup_block2"
+      body1="tool_tip"
+      body2="block2"
+      active="false"
+      torquescale="1"
+    />
+    <weld
+      name="suction_cup_block3"
+      body1="tool_tip"
+      body2="block3"
+      active="false"
+      torquescale="1"
+    />
+  </equality>
 </mujoco>
diff --git a/src/fanuc_sim/description/tool.urdf b/src/fanuc_sim/description/tool.urdf
new file mode 100644
index 0000000..055ee7a
--- /dev/null
+++ b/src/fanuc_sim/description/tool.urdf
@@ -0,0 +1,42 @@
+<?xml version="1.0" ?>
+<robot name="fanuc_tool" xmlns:xacro="http://wiki.ros.org/xacro">
+  <link name="gripper_base">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.02" />
+      <geometry>
+        <box size="0.06 0.06 0.04"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.02" />
+        <geometry>
+            <box size="0.06 0.06 0.04"/>
+        </geometry>
+        <material name="darkgrey"/>
+    </collision>
+  </link>
+  <link name="suction_cylinder">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.03" />
+      <geometry>
+        <box size="0.01 0.01 0.06"/>
+      </geometry>
+      <material name="darkgrey"/>
+    </visual>
+    <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.03" />
+        <geometry>
+            <box size="0.01 0.01 0.06"/>
+        </geometry>
+        <material name="darkgrey"/>
+    </collision>
+  </link>
+  <joint name="base_joint" type="fixed">
+    <parent link="gripper_base" />
+    <child link="suction_cylinder" />
+    <origin rpy="0 0 0" xyz="0 0 0.04" />
+  </joint>
+</robot>
diff --git a/src/fanuc_sim/description/tool.xml b/src/fanuc_sim/description/tool.xml
new file mode 100644
index 0000000..f00819a
--- /dev/null
+++ b/src/fanuc_sim/description/tool.xml
@@ -0,0 +1,44 @@
+<mujoco model="fanuc_tool">
+  <compiler angle="radian" autolimits="true" />
+  <option integrator="RK4" timestep="0.005" />
+
+  <worldbody>
+    <body
+      name="gripper_base"
+      gravcomp="1"
+      pos="0.25 0.3 0.3"
+      euler="3.1415 0 0"
+    >
+      <joint
+        type="free"
+        stiffness="0"
+        damping="0"
+        frictionloss=".001"
+        armature="0"
+      />
+      <site name="tool_attach_site" pos="0 0 0" euler="0 0 0" />
+
+      <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
+      <body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
+        <geom
+          type="cylinder"
+          size="0.01 0.03"
+          pos="0 0 0.03"
+          margin="0.01"
+          gap="0.01"
+          rgba=".1 .1 .1 1"
+        />
+        <body name="tool_tip" gravcomp="1" pos="0 0 0.055">
+          <geom
+            class="collision"
+            type="cylinder"
+            size="0.01 0.005"
+            margin="0.01"
+            gap="0.01"
+          />
+          <site name="suction_cup_tool_tip" />
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/src/fanuc_sim/objectives/activate_vacuum.xml b/src/fanuc_sim/objectives/activate_vacuum.xml
new file mode 100644
index 0000000..3cd4886
--- /dev/null
+++ b/src/fanuc_sim/objectives/activate_vacuum.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Activate Vacuum">
+  <BehaviorTree
+    ID="Activate Vacuum"
+    _description="Activate the vacuum gripper"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="MoveGripperAction"
+        position="1"
+        gripper_command_action_name="/suction_cup_controller/gripper_cmd"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Activate Vacuum" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/close_gripper.xml b/src/fanuc_sim/objectives/close_gripper.xml
deleted file mode 100644
index ac4fe63..0000000
--- a/src/fanuc_sim/objectives/close_gripper.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<root BTCPP_format="4" main_tree_to_execute="Close Gripper">
-  <BehaviorTree ID="Close Gripper" _description="Close the gripper">
-    <Control ID="Sequence" name="root">
-      <Action ID="AlwaysSuccess" />
-    </Control>
-  </BehaviorTree>
-</root>
diff --git a/src/fanuc_sim/objectives/deactivate_vacuum.xml b/src/fanuc_sim/objectives/deactivate_vacuum.xml
new file mode 100644
index 0000000..36b49c3
--- /dev/null
+++ b/src/fanuc_sim/objectives/deactivate_vacuum.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Deactivate Vacuum">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Deactivate Vacuum"
+    _description="Deactivate the vacuum gripper"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="MoveGripperAction"
+        position="0"
+        gripper_command_action_name="/suction_cup_controller/gripper_cmd"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Deactivate Vacuum" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/open_gripper.xml b/src/fanuc_sim/objectives/open_gripper.xml
deleted file mode 100644
index 9b25c0c..0000000
--- a/src/fanuc_sim/objectives/open_gripper.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
-  <!--//////////-->
-  <BehaviorTree ID="Open Gripper" _description="Open the gripper">
-    <Control ID="Sequence" name="root">
-      <Action ID="AlwaysSuccess" />
-    </Control>
-  </BehaviorTree>
-</root>
diff --git a/src/fanuc_sim/objectives/pick_up_block_with_tool.xml b/src/fanuc_sim/objectives/pick_up_block_with_tool.xml
new file mode 100644
index 0000000..a8b5bbc
--- /dev/null
+++ b/src/fanuc_sim/objectives/pick_up_block_with_tool.xml
@@ -0,0 +1,48 @@
+<root BTCPP_format="4" main_tree_to_execute="Pick Up Block With Tool">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Pick Up Block With Tool"
+    _description="Pick up the block using the suction tool"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="block2"
+        position_xyz="0;0;0.18"
+        stamped_pose="{approach_pose}"
+        orientation_xyzw="1;0;0;0"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_up_tool"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action ID="SetupMTCCurrentState" />
+      <Action
+        ID="SetupMTCMoveToPose"
+        target_pose="{approach_pose}"
+        ik_frame="tool0"
+      />
+      <Action
+        ID="SetupMTCMoveAlongFrameAxis"
+        hand_frame="tool0"
+        max_distance="0.05"
+        min_distance="0.05"
+        axis_frame="tool0"
+      />
+      <Action ID="PlanMTCTask" />
+      <Action ID="ExecuteMTCTask" />
+      <Action
+        ID="MoveGripperAction"
+        timeout="10"
+        position="1"
+        gripper_command_action_name="/suction_cup_controller/gripper_cmd"
+      />
+      <SubTree ID="Retract" _collapsed="true" distance="0.1" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick Up Block With Tool" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/pick_up_tool.xml b/src/fanuc_sim/objectives/pick_up_tool.xml
new file mode 100644
index 0000000..ef9b84d
--- /dev/null
+++ b/src/fanuc_sim/objectives/pick_up_tool.xml
@@ -0,0 +1,68 @@
+<root BTCPP_format="4" main_tree_to_execute="Pick Up Tool">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Pick Up Tool"
+    _description="Pick up the tool from the tool holder"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="tool_attach_site"
+        position_xyz="0;0;-0.1"
+        stamped_pose="{approach_pose}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_up_tool"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action ID="SetupMTCCurrentState" />
+      <Action
+        ID="SetupMTCMoveToPose"
+        target_pose="{approach_pose}"
+        ik_frame="tool0"
+        planner_interface="pro_rrt"
+      />
+      <Action
+        ID="SetupMTCMoveAlongFrameAxis"
+        hand_frame="tool0"
+        max_distance="0.095"
+        min_distance="0.095"
+        axis_frame="tool0"
+      />
+      <Action ID="PlanMTCTask" />
+      <Action ID="ExecuteMTCTask" />
+      <Action
+        ID="MoveGripperAction"
+        position="1"
+        gripper_command_action_name="/tool_attach_controller/gripper_cmd"
+        timeout="10"
+      />
+      <Control ID="Fallback">
+        <Action
+          ID="AttachTool"
+          parent_link_name="tool0"
+          tool_name="suction_gripper"
+        />
+        <Control ID="Sequence">
+          <Action
+            ID="LogMessage"
+            log_level="info"
+            message="Adding tool to the planning scene to proceed"
+          />
+          <SubTree ID="Setup Planning Scene" _collapsed="true" />
+          <Action
+            ID="AttachTool"
+            parent_link_name="tool0"
+            tool_name="suction_gripper"
+          />
+        </Control>
+      </Control>
+      <SubTree ID="Retract" _collapsed="true" distance="0.35" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick Up Tool" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml b/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml
new file mode 100644
index 0000000..c4b7c47
--- /dev/null
+++ b/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml
@@ -0,0 +1,50 @@
+<root BTCPP_format="4" main_tree_to_execute="Place Tool in Tool Holder">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Place Tool in Tool Holder"
+    _description="Puts the tool back in the tool holder"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="tool_holder_site"
+        position_xyz="0;0;-0.15"
+        stamped_pose="{approach_pose}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_up_tool"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action ID="SetupMTCCurrentState" />
+      <Action
+        ID="SetupMTCMoveToPose"
+        target_pose="{approach_pose}"
+        ik_frame="tool0"
+        planner_interface="pro_rrt"
+      />
+      <Action
+        ID="SetupMTCMoveAlongFrameAxis"
+        hand_frame="tool0"
+        max_distance="0.14"
+        axis_frame="tool0"
+        min_distance="0.14"
+      />
+      <Action ID="PlanMTCTask" />
+      <Action ID="ExecuteMTCTask" />
+      <Action ID="WaitForDuration" delay_duration="1" />
+      <Action
+        ID="MoveGripperAction"
+        position="0"
+        gripper_command_action_name="/tool_attach_controller/gripper_cmd"
+        timeout="10"
+      />
+      <Action ID="DetachTool" tool_name="suction_gripper" />
+      <SubTree ID="Retract" _collapsed="true" distance="0.35" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Place Tool in Tool Holder" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/request_teleoperation.xml b/src/fanuc_sim/objectives/request_teleoperation.xml
index e4c00ea..6e76924 100644
--- a/src/fanuc_sim/objectives/request_teleoperation.xml
+++ b/src/fanuc_sim/objectives/request_teleoperation.xml
@@ -21,10 +21,10 @@
           <Control ID="Sequence">
             <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
-              <SubTree ID="Close Gripper" />
+              <SubTree ID="Activate Vacuum" />
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
-              <SubTree ID="Open Gripper" />
+              <SubTree ID="Deactivate Vacuum" />
             </Decorator>
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
diff --git a/src/fanuc_sim/objectives/retract.xml b/src/fanuc_sim/objectives/retract.xml
new file mode 100644
index 0000000..225598d
--- /dev/null
+++ b/src/fanuc_sim/objectives/retract.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Retract">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Retract"
+    _description="Move back a distance in the tool frame"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="InitializeMTCTask"
+        controller_names="/joint_trajectory_controller"
+        task_id="retract"
+      />
+      <Action ID="SetupMTCCurrentState" skip_collision_check="true" />
+      <Action
+        ID="SetupMTCMoveAlongFrameAxis"
+        axis_frame="tool0"
+        axis_z="-1"
+        max_distance="{distance}"
+        min_distance="{distance}"
+        hand_frame="tool0"
+      />
+      <Action ID="PlanMTCTask" />
+      <Action ID="ExecuteMTCTask" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Retract">
+      <input_port name="distance" default="0.1" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/setup_planning_scene.xml b/src/fanuc_sim/objectives/setup_planning_scene.xml
new file mode 100644
index 0000000..4751fbf
--- /dev/null
+++ b/src/fanuc_sim/objectives/setup_planning_scene.xml
@@ -0,0 +1,27 @@
+<root BTCPP_format="4" main_tree_to_execute="Setup Planning Scene">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Setup Planning Scene"
+    _description="Sets the tool at its initial pose in the Planning Scene"
+    _favorite="true"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        stamped_pose="{tool_pose}"
+        reference_frame="tool_attach_site"
+      />
+      <Action
+        ID="AddToolToScene"
+        relative_pose="{tool_pose}"
+        package_name="fanuc_sim"
+        tool_urdf_file_path="description/tool.urdf"
+        tool_name="suction_gripper"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Setup Planning Scene" />
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/tool_attachment_example.xml b/src/fanuc_sim/objectives/tool_attachment_example.xml
new file mode 100644
index 0000000..3576974
--- /dev/null
+++ b/src/fanuc_sim/objectives/tool_attachment_example.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Tool Attachment Example">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Tool Attachment Example"
+    _description="Pick up a vacuum gripper, pick up an object with it, drop the object and put the tool back in its holder"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Pick Up Tool" _collapsed="true" />
+      <SubTree ID="Pick Up Block With Tool" _collapsed="true" />
+      <Action ID="WaitForDuration" delay_duration="2" />
+      <SubTree ID="Deactivate Vacuum" _collapsed="true" />
+      <SubTree ID="Place Tool in Tool Holder" _collapsed="true" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Tool Attachment Example" />
+  </TreeNodesModel>
+</root>
diff --git a/src/picknik_accessories b/src/picknik_accessories
index 6ddf657..d3458fc 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 6ddf657050f678b9eac282ade0940d827a8ea166
+Subproject commit d3458fc7142ec64f71659d756646afba70c649dd