From e123367030d445264fb6d0e9c95b72ab881b56bb Mon Sep 17 00:00:00 2001
From: Giang Nguyen <hoanggia@uni-bremen.de>
Date: Fri, 1 Apr 2022 16:45:12 +0200
Subject: [PATCH] Revert "Add option to use mimic joint (default is true)"

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

diff --git a/iai_donbot_description/robots/iai_donbot.xacro b/iai_donbot_description/robots/iai_donbot.xacro
index b75504d..0ccb84f 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" use_mimic="true">
+		<xacro:wsg_50_xacro name="gripper" parent="adapter_fwa050_wsg50_frame_out">
 			<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 8b33e1a..a4752db 100644
--- a/iai_wsg_50_description/urdf/wsg_50.urdf.xacro
+++ b/iai_wsg_50_description/urdf/wsg_50.urdf.xacro
@@ -1,273 +1,260 @@
 <?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 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"
+<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"
                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 -->
-		<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
+      	  <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>
+