From 42291f3700b39e47f010cd47c00df5f8ad7bd238 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gabriela=20=C5=A0ejnov=C3=A1?= <sejnogab@ciirc.cvut.cz>
Date: Tue, 14 Mar 2023 11:58:40 +0100
Subject: [PATCH] tiago_singlehand fixed

---
 myGym/configs/train_reach_tiago.json          |   2 +-
 .../envs/robots/tiago/tiago_pal_gripper.urdf  | 138 +++++++++---------
 2 files changed, 70 insertions(+), 70 deletions(-)

diff --git a/myGym/configs/train_reach_tiago.json b/myGym/configs/train_reach_tiago.json
index 26536484..50be5167 100644
--- a/myGym/configs/train_reach_tiago.json
+++ b/myGym/configs/train_reach_tiago.json
@@ -9,7 +9,7 @@
 "visualize" :0,
 "visgym"    :0,
 #Robot
-"robot"         :"tiago4",
+"robot"         :"tiago",
 "robot_action"  :"joints_gripper",
 "robot_init"    :[0.0, 0.4, 0.6],
 "max_velocity"  :20,
diff --git a/myGym/envs/robots/tiago/tiago_pal_gripper.urdf b/myGym/envs/robots/tiago/tiago_pal_gripper.urdf
index 9b041487..e3e92b8e 100644
--- a/myGym/envs/robots/tiago/tiago_pal_gripper.urdf
+++ b/myGym/envs/robots/tiago/tiago_pal_gripper.urdf
@@ -40,7 +40,7 @@
     <!-- this is expected to be reparented to pelvis with appropriate offset
          when imu_link is reduced by fixed joint reduction -->
     <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
-    <sensor name="imu_sensor" type="imu">
+    <sensor name="imu_sensor" type="fixed">
       <always_on>1</always_on>
       <update_rate>1000.0</update_rate>
       <imu>
@@ -103,7 +103,7 @@
   <material name="Yellow">
     <color rgba="0.96 0.88 0.14 1.0"/>
   </material>
-  <link name="base_laser_link" type="laser">
+  <link name="base_laser_link" type="fixed">
     <inertial>
       <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
       <mass value="0.28922000000"/>
@@ -117,14 +117,14 @@
       <material name="DarkGrey"/>
     </visual>
   </link>
-  <joint name="base_laser_gjoint" type="fixed">
+  <joint name="base_laser_joint" type="fixed">
     <axis xyz="0 1 0"/>
     <origin rpy="0 0 0" xyz="0.202 0.0 -0.004"/>
     <parent link="base_link"/>
     <child link="base_laser_link"/>
   </joint>
   <gazebo reference="base_laser_link">
-    <sensor name="base_laser" type="ray">
+    <sensor name="base_laser" type="fixed">
       <pose>0 0 0 0 0 0</pose>
       <update_rate>10</update_rate>
       <visualize>true</visualize>
@@ -169,14 +169,14 @@
       <material name="DarkGrey"/>
     </visual>
   </link>
-  <joint name="base_sonar_03_rjoint" type="fixed">
+  <joint name="base_sonar_03_joint" type="fixed">
     <origin rpy="0 0 2.35619449019" xyz="-0.183 0.183 0.173"/>
     <axis xyz="0 0 1"/>
     <parent link="base_link"/>
     <child link="base_sonar_03_link"/>
   </joint>
   <gazebo reference="base_sonar_03_link">
-    <sensor name="base_sonar_03" type="ray">
+    <sensor name="base_sonar_03" type="fixed">
       <pose>0 0 0 0 0 0</pose>
       <update_rate>5</update_rate>
       <visualize>false</visualize>
@@ -229,14 +229,14 @@
       <material name="DarkGrey"/>
     </visual>
   </link>
-  <joint name="base_sonar_02_rjoint" type="fixed">
+  <joint name="base_sonar_02_joint" type="fixed">
     <origin rpy="0 0 3.14159265359" xyz="-0.259 0.0 0.173"/>
     <axis xyz="0 0 1"/>
     <parent link="base_link"/>
     <child link="base_sonar_02_link"/>
   </joint>
   <gazebo reference="base_sonar_02_link">
-    <sensor name="base_sonar_02" type="ray">
+    <sensor name="base_sonar_02" type="fixed">
       <pose>0 0 0 0 0 0</pose>
       <update_rate>5</update_rate>
       <visualize>false</visualize>
@@ -289,14 +289,14 @@
       <material name="DarkGrey"/>
     </visual>
   </link>
-  <joint name="base_sonar_01_rjoint" type="fixed">
+  <joint name="base_sonar_01_joint" type="fixed">
     <origin rpy="0 0 3.92699081699" xyz="-0.183 -0.183 0.173"/>
     <axis xyz="0 0 1"/>
     <parent link="base_link"/>
     <child link="base_sonar_01_link"/>
   </joint>
   <gazebo reference="base_sonar_01_link">
-    <sensor name="base_sonar_01" type="ray">
+    <sensor name="base_sonar_01" type="fixed">
       <pose>0 0 0 0 0 0</pose>
       <update_rate>5</update_rate>
       <visualize>false</visualize>
@@ -368,7 +368,7 @@
        <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>1
   </link>
-  <joint name="base_footprint_rjoint" type="fixed">
+  <joint name="base_footprint_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0.0985"/>
     <child link="base_link"/>
     <parent link="base_footprint"/>
@@ -398,7 +398,7 @@
       <material name="Orange"/>
     </visual>
   </link>
-  <joint name="cover_rjoint" type="fixed">
+  <joint name="cover_joint" type="fixed">
     <parent link="base_footprint"/>
     <child link="base_cover_link"/>
     <origin rpy="0 0 0" xyz="0 0 0.13"/>
@@ -430,7 +430,7 @@
   <gazebo reference="base_antenna_left_link">
     <material>Gazebo/Black</material>
   </gazebo>
-  <joint name="base_antenna_left_rjoint" type="fixed">
+  <joint name="base_antenna_left_joint" type="fixed">
     <parent link="base_link"/>
     <child link="base_antenna_left_link"/>
     <origin rpy="0 0 0" xyz="-0.201 0.1062 0.195"/>
@@ -459,7 +459,7 @@
   <gazebo reference="base_antenna_right_link">
     <material>Gazebo/Black</material>
   </gazebo>
-  <joint name="base_antenna_right_rjoint" type="fixed">
+  <joint name="base_antenna_right_joint" type="fixed">
     <parent link="base_link"/>
     <child link="base_antenna_right_link"/>
     <origin rpy="0 0 0" xyz="-0.201 -0.1062 0.195"/>
@@ -477,13 +477,13 @@
         </geometry>
       </visual-->
   </link>
-  <joint name="base_imu_rjoint" type="fixed">
+  <joint name="base_imu_joint" type="fixed">
     <parent link="base_link"/>
     <child link="base_imu_link"/>
     <origin rpy="0 0 -1.57079632679" xyz="0.10171 0 0.11282"/>
   </joint>
   <gazebo reference="base_imu_link">
-    <sensor name="base_imu_sensor" type="imu">
+    <sensor name="base_imu_sensor" type="fixed">
       <always_on>1</always_on>
       <update_rate>100.0</update_rate>
       <imu>
@@ -535,7 +535,7 @@
     <kinematic>0</kinematic>
     <gravity>1</gravity>
   </link>
-  <joint name="suspension_right_rjoint" type="fixed">
+  <joint name="suspension_right_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <axis xyz="0 0 1"/>
     <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
@@ -545,7 +545,7 @@
     <dynamics damping="100"/>
     <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
   </joint>
-  <joint name="wheel_right_rjoint" type="fixed">
+  <joint name="wheel_right_joint" type="fixed">
     <parent link="suspension_right_link"/>
     <child link="wheel_right_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="0 -0.2022 0.0"/>
@@ -557,7 +557,7 @@
     <actuator name="wheel_right_motor">
       <mechanicalReduction>1.0</mechanicalReduction>
     </actuator>
-    <joint name="wheel_right_rjoint">
+    <joint name="wheel_right_joint">
       <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
       <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
     </joint>
@@ -577,7 +577,7 @@
     <minDepth>0.002</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="wheel_{side}_rjoint">
+  <gazebo reference="wheel_{side}_joint">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
   <link name="wheel_left_link">
@@ -610,7 +610,7 @@
     <kinematic>0</kinematic>
     <gravity>1</gravity>
   </link>
-  <joint name="suspension_left_rjoint" type="fixed">
+  <joint name="suspension_left_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <axis xyz="0 0 1"/>
     <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
@@ -620,7 +620,7 @@
     <dynamics damping="100"/>
     <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
   </joint>
-  <joint name="wheel_left_rjoint" type="fixed">
+  <joint name="wheel_left_joint" type="fixed">
     <parent link="suspension_left_link"/>
     <child link="wheel_left_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="0 0.2022 0.0"/>
@@ -632,7 +632,7 @@
     <actuator name="wheel_left_motor">
       <mechanicalReduction>1.0</mechanicalReduction>
     </actuator>
-    <joint name="wheel_left_rjoint">
+    <joint name="wheel_left_joint">
       <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
       <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
     </joint>
@@ -652,7 +652,7 @@
     <minDepth>0.002</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="wheel_{side}_rjoint">
+  <gazebo reference="wheel_{side}_joint">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
   <link name="caster_front_right_1_link">
@@ -695,14 +695,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_front_right_1_rjoint" type="fixed">
+  <joint name="caster_front_right_1_joint" type="fixed">
     <parent link="base_link"/>
     <child link="caster_front_right_1_link"/>
     <origin rpy="0 0 0" xyz="0.1695 -0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_front_right_2_rjoint" type="fixed">
+  <joint name="caster_front_right_2_joint" type="fixed">
     <parent link="caster_front_right_1_link"/>
     <child link="caster_front_right_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -729,10 +729,10 @@
     <minDepth>0.0005</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="caster_{side}_1_rjoint">
+  <gazebo reference="caster_{side}_1_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
-  <gazebo reference="caster_{side}_2_rjoint">
+  <gazebo reference="caster_{side}_2_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
   <link name="caster_front_left_1_link">
@@ -775,14 +775,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_front_left_1_rjoint" type="fixed">
+  <joint name="caster_front_left_1_joint" type="fixed">
     <parent link="base_link"/>
     <child link="caster_front_left_1_link"/>
     <origin rpy="0 0 0" xyz="0.1695 0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_front_left_2_rjoint" type="fixed">
+  <joint name="caster_front_left_2_joint" type="fixed">
     <parent link="caster_front_left_1_link"/>
     <child link="caster_front_left_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -809,10 +809,10 @@
     <minDepth>0.0005</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="caster_{side}_1_rjoint">
+  <gazebo reference="caster_{side}_1_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
-  <gazebo reference="caster_{side}_2_rjoint">
+  <gazebo reference="caster_{side}_2_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
   <link name="caster_back_right_1_link">
@@ -855,14 +855,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_back_right_1_rjoint" type="fixed">
+  <joint name="caster_back_right_1_joint" type="fixed">
     <parent link="base_link"/>
     <child link="caster_back_right_1_link"/>
     <origin rpy="0 0 0" xyz="-0.1735 -0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_back_right_2_rjoint" type="fixed">
+  <joint name="caster_back_right_2_joint" type="fixed">
     <parent link="caster_back_right_1_link"/>
     <child link="caster_back_right_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -889,10 +889,10 @@
     <minDepth>0.0005</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="caster_{side}_1_rjoint">
+  <gazebo reference="caster_{side}_1_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
-  <gazebo reference="caster_{side}_2_rjoint">
+  <gazebo reference="caster_{side}_2_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
   <link name="caster_back_left_1_link">
@@ -935,14 +935,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_back_left_1_rjoint" type="fixed">
+  <joint name="caster_back_left_1_joint" type="fixed">
     <parent link="base_link"/>
     <child link="caster_back_left_1_link"/>
     <origin rpy="0 0 0" xyz="-0.1735 0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_back_left_2_rjoint" type="fixed">
+  <joint name="caster_back_left_2_joint" type="fixed">
     <parent link="caster_back_left_1_link"/>
     <child link="caster_back_left_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -969,10 +969,10 @@
     <minDepth>0.0005</minDepth>
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <gazebo reference="caster_{side}_1_rjoint">
+  <gazebo reference="caster_{side}_1_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
-  <gazebo reference="caster_{side}_2_rjoint">
+  <gazebo reference="caster_{side}_2_joint">
     <implicitSpringDamper>0</implicitSpringDamper>
   </gazebo>
   <!-- Bottom fixed part of column -->
@@ -1002,7 +1002,7 @@
       </geometry>
     </collision>
   </link>
-  <joint name="torso_fixed_rjoint" type="fixed">
+  <joint name="torso_fixed_joint" type="fixed">
     <origin rpy="0 0 0" xyz="-0.062 0.0 0.193"/>
     <parent link="base_link"/>
     <child link="torso_fixed_link"/>
@@ -1033,7 +1033,7 @@
   <gazebo reference="torso_fixed_column_link">
     <material>Gazebo/DarkGrey</material>
   </gazebo>
-  <joint name="torso_fixed_column_rjoint" type="fixed">
+  <joint name="torso_fixed_column_joint" type="fixed">
     <origin rpy="0 0 0" xyz="-0.062 0 0.195"/>
     <parent link="base_link"/>
     <child link="torso_fixed_column_link"/>
@@ -1075,7 +1075,7 @@
       <material name="LightGrey"/>
     </collision>
   </link>
-  <joint name="torso_lift_rjoint" type="fixed">
+  <joint name="torso_lift_joint" type="fixed">
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.597"/>
     <parent link="torso_fixed_link"/>
     <child link="torso_lift_link"/>
@@ -1090,7 +1090,7 @@
     <actuator name="torso_lift_motor">
       <mechanicalReduction>1.0</mechanicalReduction>
     </actuator>
-    <joint name="torso_lift_rjoint">
+    <joint name="torso_lift_joint">
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
       <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
     </joint>
@@ -1116,7 +1116,7 @@
       <material name="White"/>
     </collision>
   </link>
-  <joint name="head_1_rjoint" type="fixed">
+  <joint name="head_1_joint" type="fixed">
     <parent link="torso_lift_link"/>
     <child link="head_1_link"/>
     <origin rpy="0 0 0" xyz="0.182 0.0 0.0"/>
@@ -1129,7 +1129,7 @@
     <mu1>0.9</mu1>
     <mu2>0.9</mu2>
   </gazebo>
-  <gazebo reference="head_1_rjoint">
+  <gazebo reference="head_1_joint">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
   <transmission name="head_1_trans">
@@ -1137,7 +1137,7 @@
     <actuator name="head_1_motor">
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
-    <joint name="head_1_rjoint">
+    <joint name="head_1_joint">
       <offset>0.02</offset>
       <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
@@ -1163,7 +1163,7 @@
       </geometry>
     </collision>
   </link>
-  <joint name="head_2_rjoint" type="fixed">
+  <joint name="head_2_joint" type="fixed">
     <parent link="head_1_link"/>
     <child link="head_2_link"/>
     <origin rpy="1.57079632679 0 0" xyz="0.005 0.0 0.098"/>
@@ -1177,14 +1177,14 @@
     <actuator name="head_2_motor">
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
-    <joint name="head_2_rjoint">
+    <joint name="head_2_joint">
       <offset>-0.000356197</offset>
       <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
     </joint>
   </transmission>
   <!-- frames in the center of the camera -->
-  <joint name="xtion_rjoint" type="fixed">
+  <joint name="xtion_joint" type="fixed">
     <origin rpy="-1.57629 0.0197082 -0.000892242" xyz="0.080815 0.0822042 0.00658015"/>
     <parent link="head_2_link"/>
     <child link="xtion_link"/>
@@ -1213,7 +1213,7 @@
       </collision>
       -->
   </link>
-  <joint name="xtion_optical_gjoint" type="fixed">
+  <joint name="xtion_optical_joint" type="fixed">
     <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
     <parent link="xtion_link"/>
     <child link="xtion_optical_frame"/>
@@ -1227,16 +1227,16 @@
   </link>
   <!-- frame of camera origin provided by Orbbec, need this intermediate frame for backwards compatibility with xtion_pro_live
          to reuse same camera origin -->
-  <joint name="xtion_orbbec_aux_rjoint" type="fixed">
+  <joint name="xtion_orbbec_aux_joint" type="fixed">
     <!-- This is the real origin as in the robot solidworks, it must match: 0.1015 0.095 0.008 -->
     <!-- Assuming we have 0.0908 0.08 0.0 from default camera_position, and rotated -90 deg on x.-->
     <!-- Pending confirmation from Orbbec about the position of the frames, going with old defaults-->
     <!-- <origin xyz="0.0107 0.008 0.015" rpy="0 0 0" />-->
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="xtion_link"/>
-    <child link="xtion_orbbec_aux_rjoint_frame"/>
+    <child link="xtion_orbbec_aux_joint_frame"/>
   </joint>
-  <link name="xtion_orbbec_aux_rjoint_frame">
+  <link name="xtion_orbbec_aux_joint_frame">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <mass value="0.001"/>
@@ -1244,7 +1244,7 @@
     </inertial>
   </link>
   <!-- frames of the depth sensor -->
-  <joint name="xtion_depth_rjoint" type="fixed">
+  <joint name="xtion_depth_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0.0 0.047 0.0"/>
     <parent link="xtion_rgb_frame"/>
     <child link="xtion_depth_frame"/>
@@ -1256,7 +1256,7 @@
       <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
     </inertial>
   </link>
-  <joint name="xtion_depth_optical_rjoint" type="fixed">
+  <joint name="xtion_depth_optical_joint" type="fixed">
     <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
     <parent link="xtion_depth_frame"/>
     <child link="xtion_depth_optical_frame"/>
@@ -1269,9 +1269,9 @@
     </inertial>
   </link>
   <!-- frames of the rgb sensor -->
-  <joint name="xtion_rgb_rjoint" type="fixed">
+  <joint name="xtion_rgb_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0.0 0.022 0.0"/>
-    <parent link="xtion_orbbec_aux_rjoint_frame"/>
+    <parent link="xtion_orbbec_aux_joint_frame"/>
     <child link="xtion_rgb_frame"/>
   </joint>
   <link name="xtion_rgb_frame">
@@ -1281,7 +1281,7 @@
       <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
     </inertial>
   </link>
-  <joint name="xtion_rgb_optical_rjoint" type="fixed">
+  <joint name="xtion_rgb_optical_joint" type="fixed">
     <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
     <parent link="xtion_rgb_frame"/>
     <child link="xtion_rgb_optical_frame"/>
@@ -1294,7 +1294,7 @@
     </inertial>
   </link>
   <gazebo reference="xtion_rgb_frame">
-    <sensor name="xtion_frame_sensor" type="depth">
+    <sensor name="xtion_frame_sensor" type="fixed">
       <always_on>true</always_on>
       <update_rate>30.0</update_rate>
       <camera>
@@ -1706,7 +1706,7 @@
       <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
     </inertial>
   </link>
-  <joint name="gripper_tool_rjoint" type="fixed">
+  <joint name="gripper_tool_gjoint" type="fixed">
     <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
     <parent link="arm_tool_link"/>
     <child link="gripper_tool_link"/>
@@ -1735,7 +1735,7 @@
   <gazebo reference="gripper_link">
     <material>Gazebo/White</material>
   </gazebo>
-  <joint name="gripper_rjoint" type="fixed">
+  <joint name="gripper_gjoint" type="fixed">
     <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
     <parent link="arm_tool_link"/>
     <child link="gripper_link"/>
@@ -1764,7 +1764,7 @@
   <gazebo reference="gripper_right_finger_link">
     <material>Gazebo/White</material>
   </gazebo>
-  <joint name="gripper_right_finger_rjoint" type="prismatic">
+  <joint name="gripper_right_finger_gjoint" type="prismatic">
     <origin rpy="0 0 0" xyz="0. 0 0"/>
     <parent link="gripper_link"/>
     <child link="gripper_right_finger_link"/>
@@ -1797,7 +1797,7 @@
   <gazebo reference="gripper">
     <material>Gazebo/White</material>
   </gazebo>
-  <joint name="gripper_left_finger_rjoint" type="prismatic">
+  <joint name="gripper_left_finger_gjoint" type="prismatic">
     <origin rpy="0 0 0" xyz="0. 0 0"/>
     <parent link="gripper_link"/>
     <child link="gripper_left_finger_link"/>
@@ -1808,7 +1808,7 @@
     <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
   </joint>
   <!-- Grasping frame -->
-  <joint name="gripper_grasping_frame_rjoint" type="fixed">
+  <joint name="gripper_grasping_frame_gjoint" type="fixed">
     <parent link="gripper_link"/>
     <child link="gripper"/>
     <origin rpy="-1.5708 1.5708 0" xyz="0 0 -0.12"/>
@@ -1825,7 +1825,7 @@
     <actuator name="gripper_left_finger_motor">
       <mechanicalReduction>1.0</mechanicalReduction>
     </actuator>
-    <joint name="gripper_left_finger_rjoint">
+    <joint name="gripper_left_finger_gjoint">
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
     </joint>
   </transmission>
@@ -1834,18 +1834,18 @@
     <actuator name="gripper_right_finger_motor">
       <mechanicalReduction>1.0</mechanicalReduction>
     </actuator>
-    <joint name="gripper_right_finger_rjoint">
+    <joint name="gripper_right_finger_gjoint">
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
     </joint>
   </transmission>
          <joint name="endeffector_gjoint" type="fixed">
     <parent link="gripper"/>
     <child link="endeffector"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <origin rpy="0 0 0" xyz="0.04 0 0"/>
   </joint>
   <link name="endeffector">
     <visual>
-      <origin rpy="0 0 0" xyz="0 0.0 0.0"/>
+      <origin rpy="0 0 0" xyz="0.04 0.0 0.0"/>
       <geometry>
         <sphere radius="0.01"/>
       </geometry>