controller from ur_robotiq_control.launch.py starts up

ur_robotiq_moveit.launch.py cant find gripper_gap joint

src/cartesian_controllers

@@ -1 +0,0 @@
-Subproject commit 5480fcee3a3b1ff813810775b19c795725b0fcd6

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -446,15 +446,20 @@
                     <color rgba="0.0 0.0 0.0 0.0"/>  <!-- Transparent color -->
                 </material>
             </visual>
+            <collision>
+              <geometry>
+                <sphere radius="0.001"/>  <!-- Negligible size -->
+              </geometry>
+            </collision>
         </link>
 
-        <joint name="base_to_dummy" type="fixed">
+        <joint name="${prefix}base_to_dummy" type="fixed">
             <parent link="${prefix}robotiq_140_base_link"/>
-            <child link="dummy_link"/>
+            <child link="${prefix}dummy_link"/>
             <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Adjust origin as needed -->
         </joint>
 
-        <joint name="gripper_gap" type="prismatic">
+        <joint name="${prefix}gripper_gap" type="prismatic">
             <parent link="${prefix}dummy_link"/>
             <child link="${prefix}robotiq_140_right_inner_finger_pad"/>
             <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->

src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml

@@ -1,233 +0,0 @@
-controller_manager:
-  ros__parameters:
-    force_torque_sensor_broadcaster:
-      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
-
-    scaled_joint_trajectory_controller:
-      type: ur_controllers/ScaledJointTrajectoryController
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-    cartesian_compliance_controller:
-      type: cartesian_compliance_controller/CartesianComplianceController
-
-    cartesian_force_controller:
-      type: cartesian_force_controller/CartesianForceController
-
-    cartesian_motion_controller:
-      type: cartesian_motion_controller/CartesianMotionController
-
-    motion_control_handle:
-      type: cartesian_controller_handles/MotionControlHandle
-
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
-force_torque_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: tcp_fts_sensor
-    state_interface_names:
-      - force.x
-      - force.y
-      - force.z
-      - torque.x
-      - torque.y
-      - torque.z
-    frame_id: tool0
-    topic_name: wrench
-
-scaled_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.2
-      goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
-
-cartesian_compliance_controller:
-  ros__parameters:
-
-    # This is the tip of the robot tool that you usually use for your task.
-    # For instance, it could be the drilling bit of a screwdriver or a grinding
-    # tool.  When you specify a target_wrench, i.e. some additional forces that
-    # your robot should apply to its environment, that target_wrench gets
-    # applied in this frame.
-    end_effector_link: "tool0"
-
-    # This is usually the link directly before the first actuated joint.  All
-    # controllers will build a kinematic chain from this link up to
-    # end_effector_link.  It's also the reference frame for the superposition
-    # of error components in all controllers.
-    robot_base_link: "base_link"
-
-    # This is the URDF link of your sensor. Sensor signals are assumed to be
-    # given in this frame.  It's important that this link is located somewhere
-    # between end_effector_link and robot_base_link.  If that's not the case,
-    # the controllers won't initialize and will emit an error message.
-    ft_sensor_ref_link: "tool0"
-
-    # This is the link that the robot feels compliant about. It does not need
-    # to coincide with the end_effector_link, but for many use cases, this
-    # configuration is handy. When working with a screwdriver, for instance,
-    # setting compliance_ref_link == end_effector_link makes it easy to specify
-    # downward pushing forces without generating unwanted offset moments.
-    # On the other hand, an application could benefit from yielding a little in
-    # the robot's wrist while drawing a line on a surface with a pen.
-    compliance_ref_link: "tool0"
-
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # Choose between position or velocity.  In fact, the controllers allow to
-    # set both at the same time, but not all drivers will support this.
-    # In general, position control is a little smoother due to the double
-    # time-integrated commands from the solver. If available on a robot, it
-    # should be the default.  On some drivers, the velocity interface provides
-    # faster control cycles, and hence could improve stability in
-    # contact-dominated tasks.  A drawback is that we lose one time integration
-    # step here and obtain noisier command signals in comparison to the
-    # position interface.  It's probably suitable to test both on a new robot
-    # and decide for what works best.
-    command_interfaces:
-      - position
-        #- velocity
-
-    stiffness:  # w.r.t. compliance_ref_link coordinates
-        trans_x: 500.0
-        trans_y: 500.0
-        trans_z: 500.0
-        rot_x: 20.0
-        rot_y: 20.0
-        rot_z: 20.0
-
-    solver:
-        error_scale: 0.5
-        iterations: 1
-        publish_state_feedback: True
-
-    # For all controllers, these gains are w.r.t. the robot_base_link coordinates.
-    pd_gains:
-        trans_x: {p: 0.05, d: 0.005}
-        trans_y: {p: 0.05, d: 0.005}
-        trans_z: {p: 0.05, d: 0.005}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_force_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 0.5
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 0.05}
-        trans_y: {p: 0.05}
-        trans_z: {p: 0.05}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_motion_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 1.0
-        iterations: 10
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 1.0}
-        trans_y: {p: 1.0}
-        trans_z: {p: 1.0}
-        rot_x: {p: 0.5}
-        rot_y: {p: 0.5}
-        rot_z: {p: 0.5}
-
-motion_control_handle:
-  ros__parameters:
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -25,16 +25,13 @@ controller_manager:
       type: position_controllers/JointGroupPositionController
 
     forward_gripper_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: robotiq_2f_controllers/ForwardController
 
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
+      type: robotiq_2f_controllers/GripperCommand
-
-    robotiq_gripper_joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
 
     robotiq_activation_controller:
-      type: robotiq_controllers/RobotiqActivationController
+      type: robotiq_2f_controllers/RobotiqActivationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -138,32 +135,12 @@ forward_position_controller:
 
 forward_gripper_position_controller:
   ros__parameters:
-    joints:
+    joint: $(var prefix)gripper_gap
-      - $(var tf_prefix)finger_joint
-
 
 robotiq_gripper_controller:
   ros__parameters:
-    joint: $(var tf_prefix)finger_joint
+    joint: $(var prefix)gripper_gap
-    use_effort_interface: true
+    max_gap: 0.14
-    use_speed_interface: true
-
-robotiq_gripper_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-     - $(var tf_prefix)finger_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.2
-      goal_time: 0.0
-      $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }
 
 robotiq_activation_controller:
   ros__parameters:

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -165,6 +165,9 @@ def launch_setup(context, *args, **kwargs):
             "trajectory_port:=",
             trajectory_port,
             " ",
+            "prefix:=",
+            tf_prefix,
+            " ",
         ]
     )
 
@@ -240,20 +243,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"robot_ip": robot_ip}],
     )
 
-    tool_communication_node = Node(
-        package="ur_robot_driver",
-        condition=IfCondition(use_tool_communication),
-        executable="tool_communication.py",
-        name="ur_tool_comm",
-        output="screen",
-        parameters=[
-            {
-                "robot_ip": robot_ip,
-                "tcp_port": tool_tcp_port,
-                "device_name": tool_device_name,
-            }
-        ],
-    )
 
     controller_stopper_node = Node(
         package="ur_robot_driver",
@@ -451,6 +440,15 @@ def generate_launch_description():
         have to be updated.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_fake_hardware",

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -5,8 +5,7 @@
 	<xacro:arg name="name" default="ur_manipulator"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
-	<xacro:include filename="$(find ur_robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
+	<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
-	<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
 	<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
 	<!-- the default value should raise an error in case this was called without defining the type -->
@@ -34,15 +33,9 @@
 	<xacro:arg name="script_sender_port" default="50002"/>
 	<xacro:arg name="trajectory_port" default="50003"/>
 	<!--   tool communication related parameters-->
-	<xacro:arg name="use_tool_communication" default="false" />
+	<xacro:arg name="use_tool_communication" default="true" />
-	<xacro:arg name="tool_voltage" default="0" />
+	<xacro:arg name="tool_voltage" default="24" />
-	<xacro:arg name="tool_parity" default="0" />
+	<xacro:arg name="tool_tcp_port" default="63352" />
-	<xacro:arg name="tool_baud_rate" default="115200" />
-	<xacro:arg name="tool_stop_bits" default="1" />
-	<xacro:arg name="tool_rx_idle_chars" default="1.5" />
-	<xacro:arg name="tool_tx_idle_chars" default="3.5" />
-	<xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
-	<xacro:arg name="tool_tcp_port" default="54321" />
 
 	<!-- Simulation parameters -->
 	<xacro:arg name="use_fake_hardware" default="false" />
@@ -79,12 +72,6 @@
      initial_positions="${xacro.load_yaml(initial_positions_file)}"
      use_tool_communication="$(arg use_tool_communication)"
      tool_voltage="$(arg tool_voltage)"
-     tool_parity="$(arg tool_parity)"
-     tool_baud_rate="$(arg tool_baud_rate)"
-     tool_stop_bits="$(arg tool_stop_bits)"
-     tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
-     tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
-     tool_device_name="$(arg tool_device_name)"
      tool_tcp_port="$(arg tool_tcp_port)"
      robot_ip="$(arg robot_ip)"
      script_filename="$(arg script_filename)"
@@ -103,6 +90,8 @@
     prefix="$(arg tf_prefix)"
     parent="$(arg tf_prefix)tool0"
     use_fake_hardware="$(arg use_fake_hardware)"
+    robot_ip="$(arg robot_ip)"
+    robot_port="$(arg tool_tcp_port)"
     >
     <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:robotiq_gripper>

src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro

@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur_robotiq_adapter">
-  <xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
-
-    <joint name="${prefix}ur_to_robotiq_joint" type="fixed">
-      <!-- The parent link must be read from the robot model it is attached to. -->
-      <parent link="${parent}"/>
-      <child link="${prefix}ur_to_robotiq_link"/>
-      <origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
-    </joint>
-
-    <link name="${prefix}ur_to_robotiq_link">
-        <visual>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-            <mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
-          </geometry>
-        </visual>
-        <collision>
-          <origin xyz="0 0 0" rpy="0 0 0"/>
-          <geometry>
-            <mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
-          </geometry>
-        </collision>
-        <inertial>
-          <mass value="0.01" />
-          <origin xyz="0 0 0" />
-          <inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
-        </inertial>
-    </link>
-
-    <joint name="${prefix}gripper_side_joint" type="fixed">
-      <parent link="${prefix}ur_to_robotiq_link"/>
-      <child link="${child}"/>
-      <!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
-      <origin xyz="0 0 0.011" rpy="0 0 0"/>
-    </joint>
-  </xacro:macro>
-</robot>

src/ur_robotiq_moveit_config/config/initial_positions.yaml

@@ -4,4 +4,4 @@ elbow_joint: 1.57
 wrist_1_joint: -1.57
 wrist_2_joint: -1.57
 wrist_3_joint: 0.0
-finger_joint: 20.0
+gripper_gap: 0.0

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -3,8 +3,6 @@ controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller
   - robotiq_gripper_controller
-  - robotiq_gripper_joint_trajectory_controller
-  - combined_joint_trajectory_controller
 
 
 scaled_joint_trajectory_controller:
@@ -32,16 +30,10 @@ joint_trajectory_controller:
     - wrist_2_joint
     - wrist_3_joint
 
-robotiq_gripper_controller:
-  action_ns: gripper_cmd
-  type: GripperCommand
-  default: true
-  joints:
-    - finger_joint
 
-robotiq_gripper_joint_trajectory_controller:
+robotiq_gripper_controller:
-  action_ns: follow_joint_trajectory
+  action_ns: robotiq_2f_controllers
-  type: FollowJointTrajectory
+  type: RobotiqGripperCommandController
-  default: false
+  default: true
-  joints:
+  joint: gripper_gap
-    - finger_joint
+  max_gap: 0.14

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -152,8 +152,6 @@ def launch_setup(context, *args, **kwargs):
     if use_fake_hw == "true":
         controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
         controllers_yaml["joint_trajectory_controller"]["default"] = True
-        controllers_yaml["robotiq_gripper_controller"]["default"] = False
-        controllers_yaml["robotiq_gripper_joint_trajectory_controller"]["default"] = True
 
     moveit_controllers = {
         "moveit_simple_controller_manager": controllers_yaml,

src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro

@@ -1,134 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
-  <xacro:macro name="ur_robotiq_srdf" params="name prefix">
-    <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS - When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS - When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS - Groups can also be formed by referencing to already defined group names-->
-    <group name="${prefix}${name}_manipulator">
-      <chain base_link="${prefix}base_link" tip_link="${prefix}tool0" />
-    </group>
-    <group name="${prefix}gripper">
-        <joint name="${prefix}finger_joint"/>
-        <joint name="${prefix}left_outer_finger_joint" />
-        <joint name="${prefix}left_inner_knuckle_joint" />
-        <joint name="${prefix}left_inner_finger_joint" />
-        <joint name="${prefix}right_outer_knuckle_joint" />
-        <joint name="${prefix}right_outer_finger_joint" />
-        <joint name="${prefix}right_inner_knuckle_joint" />
-        <joint name="${prefix}right_inner_finger_joint" />
-    </group>
-    <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="0" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}up" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="-1.5707" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}test_configuration" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="1.4" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.62" />
-      <joint name="${prefix}shoulder_pan_joint" value="1.54" />
-      <joint name="${prefix}wrist_1_joint" value="-1.2" />
-      <joint name="${prefix}wrist_2_joint" value="-1.6" />
-      <joint name="${prefix}wrist_3_joint" value="-0.11" />
-    </group_state>
-        <group_state name="open" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0" />
-    </group_state>
-    <group_state name="closed" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0.698132" />
-    </group_state>
-    <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
-    <end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator" />
-    <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link" />
-    <!--PASSIVE JOINT - Purpose - this element is used to mark joints that are not actuated-->
-    <passive_joint name="${prefix}left_outer_finger_joint" />
-    <passive_joint name="${prefix}left_inner_knuckle_joint" />
-    <passive_joint name="${prefix}left_inner_finger_joint" />
-    <passive_joint name="${prefix}right_outer_knuckle_joint" />
-    <passive_joint name="${prefix}right_outer_finger_joint" />
-    <passive_joint name="${prefix}right_inner_knuckle_joint" />
-    <passive_joint name="${prefix}right_inner_finger_joint" />
-    <!--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. -->
-    <!--UR-Robot-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_1_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_2_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <!--Robotiq-Gripper-->
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-
-    <!-- Weird Bug-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}upper_arm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}forearm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}flange" link2="${prefix}flange" reason="Adjacent" />
-    <disable_collisions link1="${prefix}ft_frame" link2="${prefix}ft_frame" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}tool0" reason="Adjacent" />
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-  </xacro:macro>
-</robot>

src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -14,6 +14,7 @@
         <chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
     </group>
     <group name="${prefix}gripper">
+        <joint name="${prefix}gripper_gap"/>
         <joint name="${prefix}robotiq_base_joint"/>
         <joint name="${prefix}robotiq_140_base_joint"/>
         <joint name="${prefix}finger_joint"/>
@@ -26,17 +27,14 @@
         <joint name="${prefix}right_outer_finger_joint"/>
         <joint name="${prefix}right_inner_finger_joint"/>
         <joint name="${prefix}right_inner_finger_pad_joint"/>
-    </group>
+        <joint name="${prefix}base_to_dummy"/>
-    <group name="${prefix}${name}_manipulator_combined">
-        <group name="${prefix}${name}_manipulator"/>
-        <group name="${prefix}gripper"/>
     </group>
     <!--GROUP STATES Purpose Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="${prefix}close" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0.7"/>
-    </group_state>
     <group_state name="${prefix}open" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0"/>
+        <joint name="${prefix}gripper_gap" value="0.14"/>
+    </group_state>
+    <group_state name="${prefix}closed" group="${prefix}gripper">
+        <joint name="${prefix}gripper_gap" value="0.0"/>
     </group_state>
     <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
       <joint name="${prefix}elbow_joint" value="0" />
@@ -67,6 +65,7 @@
     <!--VIRTUAL JOINT Purpose this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link"/>
     <!--PASSIVE JOINT Purpose this element is used to mark joints that are not actuated-->
+    <passive_joint name="${prefix}finger_joint"/>
     <passive_joint name="${prefix}left_inner_knuckle_joint"/>
     <passive_joint name="${prefix}left_outer_finger_joint"/>
     <passive_joint name="${prefix}left_inner_finger_joint"/>
@@ -81,90 +80,90 @@
     <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_2_link" reason="Never"/>
     <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_left_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_1_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_2_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
@@ -174,5 +173,7 @@
     <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never"/>
     <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}dummy_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}dummy_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
   </xacro:macro>
 </robot>