Setup the fanuc_sim config with a tool-changing example

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=[]]

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:

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"/>

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>

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"

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>

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>

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>

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>

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>