Change gripper motor joint to just side_gripper_joint

talos_controller_configuration/config/joint_trajectory_controllers.yaml

+torso_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - torso_1_joint
+    - torso_2_joint
+
+  constraints:
+    goal_time: &goal_time_constraint 0.6
+    stopped_velocity_tolerance: &stopped_velocity_constraint 0.05
+    torso_1_joint:
+      goal: &goal_pos_constraint 0.02
+    torso_2_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+head_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - head_1_joint
+    - head_2_joint
+
+  constraints:
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
+    head_1_joint:
+      goal: *goal_pos_constraint
+    head_2_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+left_arm_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - arm_left_1_joint
+    - arm_left_2_joint
+    - arm_left_3_joint
+    - arm_left_4_joint
+    - arm_left_5_joint
+    - arm_left_6_joint
+    - arm_left_7_joint
+  constraints:
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
+    arm_left_1_joint:
+      goal: *goal_pos_constraint
+    arm_left_2_joint:
+      goal: *goal_pos_constraint
+    arm_left_3_joint:
+      goal: *goal_pos_constraint
+    arm_left_4_joint:
+      goal: *goal_pos_constraint
+    arm_left_5_joint:
+      goal: *goal_pos_constraint
+    arm_left_6_joint:
+      goal: *goal_pos_constraint
+    arm_left_7_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+right_arm_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - arm_right_1_joint
+    - arm_right_2_joint
+    - arm_right_3_joint
+    - arm_right_4_joint
+    - arm_right_5_joint
+    - arm_right_6_joint
+    - arm_right_7_joint
+  constraints:
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
+    arm_right_1_joint:
+      goal: *goal_pos_constraint
+    arm_right_2_joint:
+      goal: *goal_pos_constraint
+    arm_right_3_joint:
+      goal: *goal_pos_constraint
+    arm_right_4_joint:
+      goal: *goal_pos_constraint
+    arm_right_5_joint:
+      goal: *goal_pos_constraint
+    arm_right_6_joint:
+      goal: *goal_pos_constraint
+    arm_right_7_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+right_gripper_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - right_gripper_joint
+
+  constraints:
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
+    right_gripper_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+left_gripper_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - left_gripper_joint
+
+  constraints:
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
+    left_gripper_joint:
+      goal: *goal_pos_constraint
+
+  stop_trajectory_duration: 0.0
+
+
 left_leg_controller:
   type: "position_controllers/JointTrajectoryController"
   joints:
@@ -8,10 +127,10 @@ left_leg_controller:
     - leg_left_5_joint
     - leg_left_6_joint
   constraints:
-    goal_time: &goal_time_constraint 0.6
-    stopped_velocity_tolerance: &stopped_velocity_constraint 0.05
+    goal_time: *goal_time_constraint
+    stopped_velocity_tolerance: *stopped_velocity_constraint
     arm_left_1_joint:
-      goal: &goal_pos_constraint 0.02
+      goal: *goal_pos_constraint
     arm_left_2_joint:
       goal: *goal_pos_constraint
     arm_left_3_joint:

talos_description/urdf/gripper/gripper_v2.urdf.xacro

@@ -4,8 +4,8 @@
 
   <!--File includes-->
   <xacro:include filename="$(find talos_description)/urdf/deg_to_rad.xacro" />
-  <!-- <xacro:include filename="$(find talos_description)/urdf/gripper.transmission.xacro" />
-  <xacro:include filename="$(find talos_description)/urdf/gripper.gazebo.xacro" /> -->
+  <xacro:include filename="$(find talos_description)/urdf/gripper/gripper.transmission.xacro" />
+  <xacro:include filename="$(find talos_description)/urdf/gripper/gripper.gazebo.xacro" />
 
 
   <xacro:macro name="talos_gripper" params="name parent">
@@ -48,9 +48,11 @@
               <inertial>
                 <origin xyz="0.02270 0.01781 -0.00977" rpy="0.00000 0.00000 0.00000"/>
                 <mass value="0.16889"/>
-                <inertia ixx="0.00015900000" ixy="-0.00007000000" ixz="0.00003800000"
-                         iyy="0.00022100000" iyz="-0.00005300000"
-                         izz="0.00026800000"/>
+                <!-- In order for Gazebo not to explode we needed to add these 0.001 values to the diagonal,
+                also the tool pal_dynamics InertiaMassVisualization gave NaN on the computation of the inertia box -->
+                <inertia ixx="${0.00015900000 + 0.001}" ixy="-0.00007000000" ixz="0.00003800000"
+                         iyy="${0.00022100000 + 0.001}" iyz="-0.00005300000"
+                         izz="${0.00026800000 + 0.001}"/>
               </inertial>
 
               <visual>
@@ -69,7 +71,7 @@
               </collision>
             </link>
 
-            <joint name="${name}_motor_double_joint" type="revolute">
+             <joint name="${name}_joint" type="revolute">
               <parent link="${name}_base_link"/>
               <child link="${name}_motor_double_link"/>
               <origin xyz="0.0 0.02025 -0.03" 
@@ -112,9 +114,9 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${1.0}" offset="0.0" />
 
-            </joint>
+            </joint> 
 
 
 
@@ -151,7 +153,7 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${-1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
             </joint>
 
 
@@ -187,11 +189,11 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${-1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
             </joint>
 
 
-            <link name="${name}_motor_single_link">
+             <link name="${name}_motor_single_link">
               <inertial>
                 <origin xyz="0.02589 -0.01284 -0.00640" rpy="0.00000 0.00000 0.00000"/>
                 <mass value="0.14765"/>
@@ -223,7 +225,7 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${-1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
             </joint>
 
 
@@ -260,9 +262,9 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${-1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
 
-            </joint>
+            </joint> 
 
 
             <link name="${name}_fingertip_3_link">
@@ -297,10 +299,12 @@
                       rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${180.00000 * deg_to_rad}"/>
               <axis xyz="1 0 0" />
               <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
-              <mimic joint="${name}_motor_double_joint" multiplier="${-1.0}" offset="0.0" />
+              <mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
             </joint>
 
 
+        <xacro:finger_mimics name="${name}" />
+        <xacro:gripper_transmission name="${name}" reduction="1.0" />
 
  </xacro:macro>