From fc2477b9c52385718e1683e188e676cb2792921e Mon Sep 17 00:00:00 2001
From: Thomason Zhou <thomason.zhou@uwaterloo.ca>
Date: Mon, 10 Mar 2025 18:08:07 +0900
Subject: [PATCH] Correct first joint of left arm

---
 openarm_two_arms/urdf/openarm.xacro           | 60 +++++++++----------
 .../urdf/openarm_two_arms.urdf.xacro          |  3 +-
 2 files changed, 32 insertions(+), 31 deletions(-)

diff --git a/openarm_two_arms/urdf/openarm.xacro b/openarm_two_arms/urdf/openarm.xacro
index 2b0d74a..9bd0109 100644
--- a/openarm_two_arms/urdf/openarm.xacro
+++ b/openarm_two_arms/urdf/openarm.xacro
@@ -13,7 +13,7 @@
         <link name="${prefix}link1">
             <visual>
                 <!--J1 v002/Part__Feature001.-->
-                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 0.0 ${reflect*0.0007}"/>
+                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 0.0 ${reflect*-0.0007}"/>
                 <geometry>
                     <mesh filename="package://openarm_two_arms/meshes/full_upper_j1v1_fj2ul1.dae"/>
                 </geometry>
@@ -27,35 +27,35 @@
             </collision>
             <inertial>
                 <mass value="0.5769283920617254"/>
-                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 2.52845e-05 0.0229621"/>
+                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 ${reflect*2.52845e-05} ${reflect*0.0229621}"/>
                 <inertia ixx="0.00034035167574060775" ixy="-2.623557985991217e-18" ixz="-6.093726095527621e-18" iyy="0.0004300058300010205" iyz="-5.09829482074806e-07" izz="0.0004802171432001996"/>
             </inertial>
         </link>
         <link name="${prefix}link2">
             <visual>
                 <!--J2 v002/Part__Feature002.-->
-                <origin rpy="0 0 0" xyz="0.0 0.0 -0.05395"/>
+                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 0.0 ${reflect*-0.05395}"/>
                 <geometry>
                     <mesh filename="package://openarm_two_arms/meshes/full_upper_j2v1_xzm1cu.dae"/>
                 </geometry>
             </visual>
             <collision>
                 <!--bound_obj__right_link2__col_J2_v1__BoundBox/col_J2_v1__BoundBox.-->
-                <origin rpy="0 0 0" xyz="0.0 0.0 0.0371236"/>
+                <origin rpy="${0 if side=='right' else pi} 0 0" xyz="0.0 0.0 ${reflect*0.0371236}"/>
                 <geometry>
                     <box size="0.0590000000149012 0.08200000000000002 0.0742471498637348"/>
                 </geometry>
             </collision>
             <inertial>
                 <mass value="0.16257504134917358"/>
-                <origin rpy="0 0 0" xyz="-0.000225878 -0.00183836 0.0278368"/>
-                <inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="-1.1282723362433474e-06" iyy="0.00010490798314778774" iyz="5.9079627839725245e-06" izz="0.00019737368685935776"/>
+                <origin rpy="0 0 0" xyz="${reflect*-0.000225878} ${reflect*-0.00183836} 0.0278368"/>
+                <inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="${reflect*-1.1282723362433474e-06}" iyy="0.00010490798314778774" iyz="${reflect*5.9079627839725245e-06}" izz="0.00019737368685935776"/>
             </inertial>
         </link>
         <link name="${prefix}link3">
             <visual>
                 <!--J3 v003/Part__Feature003.-->
-                <origin rpy="1.570796326794897 0 0" xyz="0.0 0.0987 0.02975"/>
+                <origin rpy="1.570796326794897 0 0" xyz="0.0 0.0987 ${reflect*0.02975}"/>
                 <geometry>
                     <mesh filename="package://openarm_two_arms/meshes/full_upper_j3v2_5nirfo.dae"/>
                 </geometry>
@@ -69,8 +69,8 @@
             </collision>
             <inertial>
                 <mass value="0.4201676469910031"/>
-                <origin rpy="1.5707963267948968 0 0" xyz="-0.00688022 0.0 0.0282752"/>
-                <inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="1.3304903057525575e-06" iyy="0.0002970624991387495" iyz="5.980157765704868e-07" izz="0.00032889994351244413"/>
+                <origin rpy="1.5707963267948968 0 0" xyz="${reflect*-0.00688022} 0.0 0.0282752"/>
+                <inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="${reflect*1.3304903057525575e-06}" iyy="0.0002970624991387495" iyz="${reflect*5.980157765704868e-07}" izz="0.00032889994351244413"/>
             </inertial>
         </link>
         <link name="${prefix}link4">
@@ -115,7 +115,7 @@
                 <inertia ixx="0.00031417157425298747" ixy="-1.1391566029536723e-05" ixz="-1.883474090056124e-05" iyy="0.001221711435313989" iyz="1.46725418421127e-06" izz="0.0010755135067660488"/>
             </inertial>
         </link>
-        <link name="l_J6_right_wrist">
+        <link name="${prefix}link6_right_wrist">
             <visual>
                 <!--J6 right_wrist/Part__Feature006.-->
                 <origin rpy="-2.1276679791326423 -1.5542266826921929 0" xyz="-0.0485412 -0.0860404 0.437784"/>
@@ -178,7 +178,7 @@
                 <inertia ixx="0.00023465661366788053" ixy="7.609048882006294e-05" ixz="3.088694121124684e-07" iyy="0.0005065459365215377" iyz="1.9022818028658623e-07" izz="0.0003737029250058136"/>
             </inertial>
         </link>
-        <link name="l_left_jaw_v002">
+        <link name="${prefix}link_left_jaw">
             <visual>
                 <!--left_jaw v002/Part__Feature009.-->
                 <origin rpy="0 -0.008758182894469432 0" xyz="0.665265 -0.00286677 -0.0209282"/>
@@ -199,7 +199,7 @@
                 <inertia ixx="1.1771768742932353e-05" ixy="-2.389928166589898e-06" ixz="3.999292204534159e-06" iyy="2.3837354480794824e-05" iyz="3.494431877242991e-07" izz="3.0474590214086944e-05"/>
             </inertial>
         </link>
-        <link name="l_right_jaw_v002">
+        <link name="${prefix}link_right_jaw">
             <visual>
                 <!--right_jaw v002/Part__Feature010.-->
                 <origin rpy="0 -0.008758182898001485 0" xyz="0.665265 -0.00286677 -0.175928"/>
@@ -220,69 +220,69 @@
                 <inertia ixx="1.1561293087621594e-05" ixy="2.537718468008212e-06" ixz="-3.6591812257702003e-06" iyy="2.370165527871047e-05" iyz="4.848038235889027e-07" izz="3.039652946542647e-05"/>
             </inertial>
         </link>
-        <joint name="right_rev1" type="revolute">
+        <joint name="${prefix}rev1" type="revolute">
             <parent link="${prefix}link1"/>
             <child link="${prefix}link2"/>
-            <origin rpy="0 0 0" xyz="0.0 0.0 0.05325"/>
+            <origin rpy="${0 if side=='right' else -pi} ${0 if side=='right' else -pi} 0" xyz="0.0 0.0 ${reflect*0.05325}"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="0.0"/>
         </joint>
-        <joint name="right_rev2" type="revolute">
+        <joint name="${prefix}rev2" type="revolute">
             <parent link="${prefix}link2"/>
             <child link="${prefix}link3"/>
-            <origin rpy="-1.5707963267948968 0 0" xyz="0.0 -0.02975 0.04475"/>
+            <origin rpy="-1.5707963267948968 0 0" xyz="0.0 ${reflect*-0.02975} 0.04475"/>
             <axis xyz="0 0 1"/>
-            <limit effort="0.0" lower="0.0" upper="3.141592653589793" velocity="0.0"/>
+            <limit effort="0.0" lower="${0.0 if side=='right' else -pi}" upper="${pi if side=='right' else 0.0}" velocity="0.0"/>
         </joint>
-        <joint name="right_rev3" type="revolute">
+        <joint name="${prefix}rev3" type="revolute">
             <parent link="${prefix}link3"/>
             <child link="${prefix}link4"/>
-            <origin rpy="-1.5707963267949054 0 -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
+            <origin rpy="${reflect*-1.5707963267949054} 0 ${reflect*-1.562038143900564}" xyz="${reflect*-0.0612477} -0.000536432 ${reflect*0.02975}"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" velocity="0.0"/>
         </joint>
-        <joint name="right_rev4" type="revolute">
+        <joint name="${prefix}rev4" type="revolute">
             <parent link="${prefix}link4"/>
             <child link="${prefix}link5"/>
             <origin rpy="-0.20701608758495998 -1.5707963267948566 -2.937419385117548" xyz="0.0297547 0.0 -0.24175"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-0.3490658503988659" upper="2.792526803190927" velocity="0.0"/>
         </joint>
-        <joint name="right_rev5" type="revolute">
+        <joint name="${prefix}rev5" type="revolute">
             <parent link="${prefix}link5"/>
-            <child link="l_J6_right_wrist"/>
+            <child link="${prefix}link6_right_wrist"/>
             <origin rpy="1.5707963267948473 -0.5569332500492129 1.556730325338251" xyz="-0.133937 0.00188408 -0.0297547"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="0.0"/>
         </joint>
-        <joint name="right_rev6" type="revolute">
-            <parent link="l_J6_right_wrist"/>
+        <joint name="${prefix}rev6" type="revolute">
+            <parent link="${prefix}link6_right_wrist"/>
             <child link="${prefix}link7"/>
             <origin rpy="-1.5707963268024898 -1.5567303253382425 -0.5569332500461536" xyz="-0.0187648 -0.0301352 -0.12105"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="0.0"/>
         </joint>
-        <joint name="right_rev7" type="revolute">
+        <joint name="${prefix}rev7" type="revolute">
             <parent link="${prefix}link7"/>
             <child link="${prefix}link8"/>
             <origin rpy="-1.5707963267950005 -0.00875904933536772 -0.014066001454933467" xyz="-0.000217313 -0.0154485 0.0355"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="-0.9599310885968813" upper="0.9599310885968813" velocity="0.0"/>
         </joint>
-        <joint name="right_left_pris1" type="prismatic">
+        <joint name="${prefix}left_pris1" type="prismatic">
             <parent link="${prefix}link8"/>
-            <child link="l_left_jaw_v002"/>
+            <child link="${prefix}link_left_jaw"/>
             <origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="0.0" upper="0.045" velocity="0.0"/>
         </joint>
-        <joint name="right_right_pris2" type="prismatic">
+        <joint name="${prefix}right_pris2" type="prismatic">
             <parent link="${prefix}link8"/>
-            <child link="l_right_jaw_v002"/>
+            <child link="${prefix}link_right_jaw"/>
             <origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
             <axis xyz="0 0 1"/>
             <limit effort="0.0" lower="0.0" upper="0.045" velocity="0.0"/>
-            <mimic joint="right_left_pris1" multiplier="-1.0" offset="0.0"/>
+            <mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/>
         </joint>
     </xacro:macro>
 </robot>
diff --git a/openarm_two_arms/urdf/openarm_two_arms.urdf.xacro b/openarm_two_arms/urdf/openarm_two_arms.urdf.xacro
index 15ee166..3ff15a7 100644
--- a/openarm_two_arms/urdf/openarm_two_arms.urdf.xacro
+++ b/openarm_two_arms/urdf/openarm_two_arms.urdf.xacro
@@ -5,7 +5,8 @@
   <xacro:include filename="openarm.xacro"/>
   <xacro:include filename="openarm_two_arms_pedestal.urdf"/>
   <xacro:openarm prefix="right_" side="right"/>
-  <xacro:include filename="openarm_left.urdf"/>
+  <xacro:openarm prefix="left_" side="left"/>
+  <!-- <xacro:include filename="openarm_left.urdf"/> -->
 
   <joint name="dummy_joint" type="fixed">
     <parent link="world"/>