From 9593a8683ab5cf7c9526378556bb10ac7f3acb7a Mon Sep 17 00:00:00 2001
From: Giang Nguyen <hoanggia@uni-bremen.de>
Date: Fri, 1 Apr 2022 11:33:41 +0200
Subject: [PATCH] Add option to use mimic joint (default is true)

---
 .../robots/iai_donbot.xacro                   |   2 +-
 iai_wsg_50_description/urdf/wsg_50.urdf.xacro | 527 +++++++++---------
 2 files changed, 271 insertions(+), 258 deletions(-)

diff --git a/iai_donbot_description/robots/iai_donbot.xacro b/iai_donbot_description/robots/iai_donbot.xacro
index 0ccb84f..b75504d 100644
--- a/iai_donbot_description/robots/iai_donbot.xacro
+++ b/iai_donbot_description/robots/iai_donbot.xacro
@@ -85,7 +85,7 @@
 		<xacro:adapter_fwa050_wsg50_xacro parent="fwk_fwa_050_frame_out" prefix="">
 			<origin xyz="0 0 0.0" rpy="0 0 0" />
 		</xacro:adapter_fwa050_wsg50_xacro>
-		<xacro:wsg_50_xacro name="gripper" parent="adapter_fwa050_wsg50_frame_out">
+		<xacro:wsg_50_xacro name="gripper" parent="adapter_fwa050_wsg50_frame_out" use_mimic="true">
 			<origin xyz="0 0 0" rpy="0 0 3.14159265359" />
 		</xacro:wsg_50_xacro>
 		<link name="camera_holder_link">
diff --git a/iai_wsg_50_description/urdf/wsg_50.urdf.xacro b/iai_wsg_50_description/urdf/wsg_50.urdf.xacro
index a4752db..8b33e1a 100644
--- a/iai_wsg_50_description/urdf/wsg_50.urdf.xacro
+++ b/iai_wsg_50_description/urdf/wsg_50.urdf.xacro
@@ -1,260 +1,273 @@
 <?xml version="1.0"?>
-<robot  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-        xmlns:xacro="http://www.ros.org/wiki/xacro"
-	name="wsg_50">
-
-<xacro:macro name="wsg_50_xacro" params="name parent *origin">
-
- <joint name="${name}_anterior_gripper_joint" type="fixed">
-    <xacro:insert_block name="origin"/>
-    <parent link="${parent}"/>
-    <child link="${name}_base_link"/>
- </joint>
-
-<!-- BASE LINK -->
- <link name="${name}_base_link">
-      <inertial>
-          <mass value="0.25" />
-          <origin xyz="0 0 0" />
-     	  <!--inertia ixx="1.0" ixy="0.0" ixz="0.0"
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:xacro="http://www.ros.org/wiki/xacro" name="wsg_50">
+
+	<xacro:macro name="wsg_50_xacro" params="name parent *origin use_mimic:=true">
+
+		<joint name="${name}_anterior_gripper_joint" type="fixed">
+			<xacro:insert_block name="origin" />
+			<parent link="${parent}" />
+			<child link="${name}_base_link" />
+		</joint>
+
+		<!-- BASE LINK -->
+		<link name="${name}_base_link">
+			<inertial>
+				<mass value="0.25" />
+				<origin xyz="0 0 0" />
+				<!--inertia ixx="1.0" ixy="0.0" ixz="0.0"
                iyy="100.0" iyz="0.0" 
                izz="1.0" /-->
-      	  <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
-      </inertial>
-
-      <visual>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1"/>
-	     <!--box size="0.146 0.05 0.0735"/-->
-          </geometry>
-          <material name="grey">
-		<color rgba="0.5 0.5 0.5 1"/>
-      	  </material>
-      </visual>
-
-      <collision>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-	     <!--box size="0.146 0.05 0.0735"/-->
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1"/>
-         </geometry>
-     </collision>
-  </link>
-
-  <gazebo reference="${name}_base_link">
-    <material>Gazebo/Grey</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-
-  <!-- GRIPPER LEFT -->
-
-  <joint name="${name}_base_gripper_left_joint" type="prismatic">
-     <mimic joint ="${name}_joint" multiplier="-0.5"/>
-     <limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="0.42"/>
-     <origin xyz="0 0 0" rpy="0 0 0" />      <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
-     <parent link="${name}_base_link"/>
-     <child link="${name}_gripper_left_link" />
-     <dynamics friction="100" damping="100" />
-     <axis xyz="1 0 0"/>
-     <!-- <limit effort="100" velocity="100"/> -->
-  </joint>
-
-  <transmission name="${name}_base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
-     <actuator name="${name}_base_motor_left" />
-     <joint name="${name}_base_gripper_left_joint" />
-     <mechanicalReduction>1</mechanicalReduction>
-     <motorTorqueConstant>1</motorTorqueConstant>
-  </transmission>
-
-  <link name="${name}_gripper_left_link">
-      <inertial>
-          <mass value="0.1" />
-          <origin xyz="0 0 0" />
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
-               iyy="1.0" iyz="0.0" 
-               izz="1.0" />
-      </inertial>
-
-      <visual>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
-	     <!--box size="0.0175 0.03 0.024"/-->
-          </geometry>
-          <material name="black">
-		<color rgba="0 0 0 1"/>
-      	  </material>
-      </visual>
-
-      <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-	     <!--box size="0.0175 0.03 0.024"/-->
-             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
-         </geometry>
-     </collision>
-     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
-  </link>
-
-  <gazebo reference="${name}_gripper_left_link">
-    <material>Gazebo/Blue</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-
-
-  <!-- LEFT FINGER -->
-
-  <joint name="${name}_guide_joint_finger_left" type="fixed">
-     <origin xyz="0 0 0.023" rpy="0 0 0" />
-     <parent link="${name}_gripper_left_link"/>
-     <child link="${name}_finger_left_link" />
-     <dynamics friction="100" damping="100" />
-     <axis xyz="1 0 0"/>
-     <!-- <limit effort="100" velocity="100"/> -->
-  </joint>
-
-  <link name="${name}_finger_left_link">
-      <inertial>
-          <mass value="0.1" />
-          <origin xyz="0 0 0" />
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
-               iyy="1.0" iyz="0.0" 
-               izz="1.0" />
-      </inertial>
-
-      <visual>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
-          </geometry>
-          <material name="black">
-		<color rgba="0 0 0 1"/>
-      	  </material>
-      </visual>
-
-      <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
-         </geometry>
-     </collision>
-     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
-  </link>
-
-  <gazebo reference="${name}_finger_left">
-    <material>Gazebo/Blue</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-
-
-  <!-- GRIPPER RIGHT -->
-
-  <joint name="${name}_joint" type="prismatic">
-     <limit lower="0.0065" upper="0.109" effort="1.0" velocity="0.42"/>
-     <origin xyz="0 0 0" rpy="0 0 3.14159" />
-     <parent link="${name}_gripper_left_link"/>
-     <child link="${name}_gripper_right_link" />
-     <axis xyz="-1 0 0"/>
-     <dynamics friction="100" damping="100" />
-     <!-- <limit effort="100" velocity="100"/>-->
-  </joint>
-
-  <transmission name="${name}_base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
-     <actuator name="${name}_base_motor_right" />
-     <joint name="${name}_base_gripper_right_joint" />
-     <mechanicalReduction>1</mechanicalReduction>
-     <motorTorqueConstant>1</motorTorqueConstant>
-  </transmission>
-	
-    <link name="${name}_gripper_right_link">
-      <inertial>
-          <mass value="0.1" />
-          <origin xyz="0 0 0" />
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
-               iyy="1.0" iyz="0.0" 
-               izz="1.0" />
-      </inertial>
-
-      <visual>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
-	     <!--box size="0.0175 0.03 0.024"/-->
-          </geometry>
-          <material name="black">
-		<color rgba="0 0 0 1"/>
-      	  </material>
-      </visual>
-
-      <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-	     <!--box size="0.0175 0.03 0.024"/-->
-             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
-         </geometry>
-     </collision>
-     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
-  </link>
-
-  <gazebo reference="gripper_right">
-    <material>Gazebo/White</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-
-  <!-- RIGHT FINGER -->
-
-  <joint name="${name}_guide_joint_finger_right" type="fixed">
-     <origin xyz="0 0 0.023" rpy="0 0 0" />
-     <parent link="${name}_gripper_right_link"/>
-     <child link="${name}_finger_right_link" />
-  </joint>
-
-  <link name="${name}_finger_right_link">
-      <inertial>
-          <mass value="0.1" />
-          <origin xyz="0 0 0" />
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
-               iyy="1.0" iyz="0.0" 
-               izz="1.0" />
-      </inertial>
-
-      <visual>
-          <origin xyz="0.0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
-          </geometry>
-          <material name="black">
-		<color rgba="0 0 0 1"/>
-      	  </material>
-      </visual>
-
-      <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
-         </geometry>
-     </collision>
-     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
-  </link>
-
-  <gazebo reference="${name}_finger_right">
-    <material>Gazebo/White</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-
-
-  <link name="${name}_tool_frame"/>
-
-  <joint name="${name}_tool_frame_joint" type="fixed">
-     <origin xyz="0 0 0.144" rpy="0 0 0" />
-     <parent link="${name}_base_link"/>
-     <child link="${name}_tool_frame" />
-  </joint>
-
-</xacro:macro>
-
-</robot>
-
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+			</inertial>
+
+			<visual>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1" />
+					<!--box size="0.146 0.05 0.0735"/-->
+				</geometry>
+				<material name="grey">
+					<color rgba="0.5 0.5 0.5 1" />
+				</material>
+			</visual>
+
+			<collision>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<!--box size="0.146 0.05 0.0735"/-->
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1" />
+				</geometry>
+			</collision>
+		</link>
+
+		<gazebo reference="${name}_base_link">
+			<material>Gazebo/Grey</material>
+			<turnGravityOff>false</turnGravityOff>
+		</gazebo>
+
+		<!-- GRIPPER LEFT -->
+		<xacro:if value="${use_mimic}">
+			<joint name="${name}_base_gripper_left_joint" type="prismatic">
+				<mimic joint="${name}_joint" multiplier="-0.5" />
+				<limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="0.42" />
+				<origin xyz="0 0 0" rpy="0 0 0" /> <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
+				<parent link="${name}_base_link" />
+				<child link="${name}_gripper_left_link" />
+				<dynamics friction="100" damping="100" />
+				<axis xyz="1 0 0" />
+				<!-- <limit effort="100" velocity="100"/> -->
+			</joint>
+
+			<transmission name="${name}_base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
+				<actuator name="${name}_base_motor_left" />
+				<joint name="${name}_base_gripper_left_joint" />
+				<mechanicalReduction>1</mechanicalReduction>
+				<motorTorqueConstant>1</motorTorqueConstant>
+			</transmission>
+		</xacro:if>
+
+		<xacro:unless value="${use_mimic}">
+			<joint name="${name}_base_gripper_left_joint" type="prismatic">
+				<limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="0.42" />
+				<origin xyz="0 0 0" rpy="0 0 0" /> <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
+				<parent link="${name}_base_link" />
+				<child link="${name}_gripper_left_link" />
+				<dynamics friction="100" damping="100" />
+				<axis xyz="1 0 0" />
+				<!-- <limit effort="100" velocity="100"/> -->
+			</joint>
+		</xacro:unless>
+
+		<link name="${name}_gripper_left_link">
+			<inertial>
+				<mass value="0.1" />
+				<origin xyz="0 0 0" />
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+			</inertial>
+
+			<visual>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001" />
+					<!--box size="0.0175 0.03 0.024"/-->
+				</geometry>
+				<material name="black">
+					<color rgba="0 0 0 1" />
+				</material>
+			</visual>
+
+			<collision>
+				<origin xyz="0 0 0" rpy="0 0 0" />
+				<geometry>
+					<!--box size="0.0175 0.03 0.024"/-->
+					<mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+			</collision>
+			<contact_coefficients kd="1.0" kp="1000.0" mu="0" />
+		</link>
+
+		<gazebo reference="${name}_gripper_left_link">
+			<material>Gazebo/Blue</material>
+			<turnGravityOff>false</turnGravityOff>
+		</gazebo>
+
+
+		<!-- LEFT FINGER -->
+
+		<joint name="${name}_guide_joint_finger_left" type="fixed">
+			<origin xyz="0 0 0.023" rpy="0 0 0" />
+			<parent link="${name}_gripper_left_link" />
+			<child link="${name}_finger_left_link" />
+			<dynamics friction="100" damping="100" />
+			<axis xyz="1 0 0" />
+			<!-- <limit effort="100" velocity="100"/> -->
+		</joint>
+
+		<link name="${name}_finger_left_link">
+			<inertial>
+				<mass value="0.1" />
+				<origin xyz="0 0 0" />
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+			</inertial>
+
+			<visual>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+				<material name="black">
+					<color rgba="0 0 0 1" />
+				</material>
+			</visual>
+
+			<collision>
+				<origin xyz="0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+			</collision>
+			<contact_coefficients kd="1.0" kp="1000.0" mu="0" />
+		</link>
+
+		<gazebo reference="${name}_finger_left">
+			<material>Gazebo/Blue</material>
+			<turnGravityOff>false</turnGravityOff>
+		</gazebo>
+
+
+		<!-- GRIPPER RIGHT -->
+		<xacro:if value="${use_mimic}">
+			<joint name="${name}_joint" type="prismatic">
+				<limit lower="0.0065" upper="0.109" effort="1.0" velocity="0.42" />
+				<origin xyz="0 0 0" rpy="0 0 3.14159" />
+				<parent link="${name}_gripper_left_link" />
+				<child link="${name}_gripper_right_link" />
+				<axis xyz="-1 0 0" />
+				<dynamics friction="100" damping="100" />
+				<!-- <limit effort="100" velocity="100"/>-->
+			</joint>
+
+			<transmission name="${name}_base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
+				<actuator name="${name}_base_motor_right" />
+				<joint name="${name}_base_gripper_right_joint" />
+				<mechanicalReduction>1</mechanicalReduction>
+				<motorTorqueConstant>1</motorTorqueConstant>
+			</transmission>
+		</xacro:if>
+
+		<xacro:unless value="${use_mimic}">
+			<joint name="${name}_base_gripper_right_joint" type="prismatic">
+				<limit lower="0.0027" upper="0.055" effort="1.0" velocity="0.42" />
+				<origin xyz="0 0 0" rpy="0 0 3.14159" />
+				<parent link="${name}_base_link" />
+				<child link="${name}_gripper_right_link" />
+				<axis xyz="-1 0 0" />
+				<dynamics friction="100" damping="100" />
+				<!-- <limit effort="100" velocity="100"/>-->
+			</joint>
+		</xacro:unless>
+
+		<link name="${name}_gripper_right_link">
+			<inertial>
+				<mass value="0.1" />
+				<origin xyz="0 0 0" />
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+			</inertial>
+
+			<visual>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001" />
+					<!--box size="0.0175 0.03 0.024"/-->
+				</geometry>
+				<material name="black">
+					<color rgba="0 0 0 1" />
+				</material>
+			</visual>
+
+			<collision>
+				<origin xyz="0 0 0" rpy="0 0 0" />
+				<geometry>
+					<!--box size="0.0175 0.03 0.024"/-->
+					<mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+			</collision>
+			<contact_coefficients kd="1.0" kp="1000.0" mu="0" />
+		</link>
+
+		<gazebo reference="gripper_right">
+			<material>Gazebo/White</material>
+			<turnGravityOff>false</turnGravityOff>
+		</gazebo>
+
+		<!-- RIGHT FINGER -->
+
+		<joint name="${name}_guide_joint_finger_right" type="fixed">
+			<origin xyz="0 0 0.023" rpy="0 0 0" />
+			<parent link="${name}_gripper_right_link" />
+			<child link="${name}_finger_right_link" />
+		</joint>
+
+		<link name="${name}_finger_right_link">
+			<inertial>
+				<mass value="0.1" />
+				<origin xyz="0 0 0" />
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+			</inertial>
+
+			<visual>
+				<origin xyz="0.0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+				<material name="black">
+					<color rgba="0 0 0 1" />
+				</material>
+			</visual>
+
+			<collision>
+				<origin xyz="0 0 0" rpy="0 0 0" />
+				<geometry>
+					<mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001" />
+				</geometry>
+			</collision>
+			<contact_coefficients kd="1.0" kp="1000.0" mu="0" />
+		</link>
+
+		<gazebo reference="${name}_finger_right">
+			<material>Gazebo/White</material>
+			<turnGravityOff>false</turnGravityOff>
+		</gazebo>
+
+
+		<link name="${name}_tool_frame" />
+
+		<joint name="${name}_tool_frame_joint" type="fixed">
+			<origin xyz="0 0 0.144" rpy="0 0 0" />
+			<parent link="${name}_base_link" />
+			<child link="${name}_tool_frame" />
+		</joint>
+
+	</xacro:macro>
+
+</robot>
\ No newline at end of file