This commit is allowing to have pids gains making Talos less springy.

Moreover it removes the call to moveit and several serves useless for
the SoT.

config/pids.yaml

+gains:
+  leg_left_1_joint:  &leg_1_gains {p:  20000, d: 40, i:  5, i_clamp:   9000, torque_clamp: 6600}
+  leg_left_2_joint:  &leg_2_gains {p:  20000, d: 40, i:  5, i_clamp:   9400, torque_clamp: 6600}
+  leg_left_3_joint:  &leg_3_gains {p:  20000, d: 40, i:  5, i_clamp:   9400, torque_clamp: 6600}
+  leg_left_4_joint:  &leg_4_gains {p:  20000, d: 40, i:  5, i_clamp:   9500, torque_clamp: 6000}
+  leg_left_5_joint:  &leg_5_gains {p:  20000, d: 40, i:  5, i_clamp:   9400, torque_clamp: 6600}
+  leg_left_6_joint:  &leg_6_gains {p:  20000, d: 40, i:  5, i_clamp:   9000, torque_clamp: 6000}
+  leg_right_1_joint: *leg_1_gains
+  leg_right_2_joint: *leg_2_gains
+  leg_right_3_joint: *leg_3_gains
+  leg_right_4_joint: *leg_4_gains
+  leg_right_5_joint: *leg_5_gains
+  leg_right_6_joint: *leg_6_gains
+
+
+  head_1_joint:                   {p: 300,  d: 0.1, i: 1, i_clamp: 5, torque_clamp: 8}
+  head_2_joint:                   {p: 300,  d: 0.1, i: 1, i_clamp: 1.5, torque_clamp: 8}
+
+  torso_1_joint:                  {p: 10000, d: 10, i: 1, i_clamp: 10, torque_clamp: 100}
+  torso_2_joint:                  {p: 10000, d: 10, i: 1, i_clamp: 10, torque_clamp: 100}
+
+  arm_right_1_joint: &arm_1_gains {p: 10000, d: 0, i: 0,  i_clamp: 14, torque_clamp: 150}
+  arm_right_2_joint: &arm_2_gains {p: 10000, d: 0, i: 0,  i_clamp: 14, torque_clamp: 150}
+  arm_right_3_joint: &arm_3_gains {p: 5000, d: 0, i: 0,  i_clamp: 9, torque_clamp: 100}
+  arm_right_4_joint: &arm_4_gains {p: 10000, d: 0, i: 0,  i_clamp: 9, torque_clamp: 100}
+  arm_right_5_joint: &arm_5_gains {p: 3000,  d: 0, i: 0, i_clamp: 5, torque_clamp: 50}
+  arm_right_6_joint: &arm_6_gains {p: 3000,  d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
+  arm_right_7_joint: &arm_7_gains {p: 3000,  d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
+  arm_left_1_joint:  *arm_1_gains
+  arm_left_2_joint:  *arm_2_gains
+  arm_left_3_joint:  *arm_3_gains
+  arm_left_4_joint:  *arm_4_gains
+  arm_left_5_joint:  *arm_5_gains
+  arm_left_6_joint:  *arm_6_gains
+  arm_left_7_joint:  *arm_7_gains
+
+  gripper_left_joint:  {p: 1000, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+  gripper_right_joint: {p: 1000, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+
+  #https://answers.ros.org/question/283537/how-to-do-mimic-joints-that-work-in-gazebo/
+  gripperleft_inner_double_joint: &gripper_gains {p: 20, d: 0.0, i: 0.0, i_clamp: 0.2, antiwindup: false}
+  gripper_left_fingertip_1_joint: *gripper_gains
+  gripper_left_fingertip_2_joint: *gripper_gains
+  gripper_left_motor_single_joint: *gripper_gains
+  gripper_left_fingertip_3_joint: *gripper_gains
+  gripper_left_inner_single_joint: *gripper_gains
+  gripper_left_inner_double_joint: *gripper_gains
+
+  gripper_right_inner_double_joint: *gripper_gains
+  gripper_right_fingertip_1_joint: *gripper_gains
+  gripper_right_fingertip_2_joint: *gripper_gains
+  gripper_right_motor_single_joint: *gripper_gains
+  gripper_right_fingertip_3_joint: *gripper_gains
+  gripper_right_inner_single_joint: *gripper_gains

launch/talos_bringup.launch

+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <!-- Load default controllers -->
+  <include file="$(find talos_controller_configuration)/launch/bringup_controllers.launch" />
+
+  <!-- Robot state publisher -->
+  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="100.0" />
+    <param name="tf_prefix" type="string" value="" />
+  </node>
+
+</launch>

launch/talos_gazebo_alone.launch

@@ -45,14 +45,4 @@
     </include>
   </group>
 
-
-  <!-- spawn robot in simulation -->
-  <!-- <include file="$(find talos_gazebo)/launch/talos_spawn.launch"> -->
-  <!--   <arg name="robot"  value="$(arg robot)"/> -->
-  <!--   <arg name="gzpose" value="$(arg gzpose)"/> -->
-  <!-- </include> -->
-
-  <!-- default controllers -->
-  <!-- <include file="$(find talos_controller_configuration)/launch/talos_default_controllers.launch"/> -->
-  <!-- <include file="$(find talos_bringup)/launch/talos_bringup.launch" /> -->
 </launch>

launch/talos_gazebo_spawn_hs.launch

@@ -10,7 +10,7 @@
 
   <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
   
-  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <include file="$(find talos_data)/launch/talos_spawn.launch">
   <arg name="robot"  value="$(arg robot)"/>
   <arg name="gzpose" value="$(arg gzpose)"/>
   </include>

launch/talos_gazebo_spawn_hs_wide.launch

@@ -10,7 +10,7 @@
 
   <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
   
-  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <include file="$(find talos_data)/launch/talos_spawn.launch">
   <arg name="robot"  value="$(arg robot)"/>
   <arg name="gzpose" value="$(arg gzpose)"/>
   </include>

launch/talos_spawn.launch

@@ -12,7 +12,7 @@
 
 
   <!-- PID gains -->
-  <rosparam command="load" file="$(find talos_hardware_gazebo)/config/pids.yaml"/>
+  <rosparam command="load" file="$(find talos_data)/config/pids.yaml"/>
 
   <!-- Spawn robot in Gazebo -->
   <node pkg="gazebo_ros" type="spawn_model" name="spawn_model"

scripts/start_talos_gazebo.py

@@ -6,6 +6,8 @@ import os
 import rospy
 import time
 import roslaunch
+import rospkg
+
 from std_srvs.srv import Empty
 
 # Start roscore
@@ -13,14 +15,18 @@ import subprocess
 roscore = subprocess.Popen('roscore')
 time.sleep(1)
 
+# Get the path to talos_data
+arospack = rospkg.RosPack()
+talos_data_path = arospack.get_path('talos_data')
+
 # Start talos_gazebo
 rospy.init_node('starting_talos_gazebo', anonymous=True)
 uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
 roslaunch.configure_logging(uuid)
 
-cli_args = ['/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch',
+cli_args = [talos_data_path+'/launch/talos_gazebo_alone.launch',
             'world:=empty_forced',
-            'enable_leg_passive:=false'
+            'enable_leg_passive:=true'
            ]
 roslaunch_args = cli_args[1:]
 roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]
@@ -35,8 +41,10 @@ gazebo_pause_physics()
 
 time.sleep(5)
 # Spawn talos model in gazebo
-launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs.launch"])
-#launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs_wide.launch"])
+#launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+#                                                          [talos_data_path+'/launch/talos_gazebo_spawn_hs.launch'])
+launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+                                                          [talos_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch'])
 launch_gazebo_spawn_hs.start()
 rospy.loginfo("talos_gazebo_spawn_hs started")
 
@@ -46,12 +54,15 @@ gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
 gazebo_unpause_physics()
 
 # Start roscontrol
-launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_bringup/launch/talos_bringup.launch"])
+launch_bringup = roslaunch.parent.ROSLaunchParent(uuid,
+                                                  [talos_data_path+'/launch/talos_bringup.launch'])
 launch_bringup.start()
 rospy.loginfo("talos_bringup started")
 
 # Start sot
-launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch"])
+roscontrol_sot_talos_path=arospack.get_path('roscontrol_sot_talos')
+launch_roscontrol_sot_talos =roslaunch.parent.ROSLaunchParent(uuid,
+                                                              [roscontrol_sot_talos_path+'/launch/sot_talos_controller_gazebo.launch'])
 launch_roscontrol_sot_talos.start()
 rospy.loginfo("roscontrol_sot_talos started")