Add config pids_stiff.yaml file.

config/pids_spring.yaml

+gains:
+  leg_left_1_joint:  &leg_1_gains {p:  5000, d: 20, i:  5, i_clamp:   7, torque_clamp: 60}
+  leg_left_2_joint:  &leg_2_gains {p:  5000, d: 20, i:  5, i_clamp:   14, torque_clamp: 160}
+  leg_left_3_joint:  &leg_3_gains {p:  5000, d: 20, i:  5, i_clamp:   14, torque_clamp: 160}
+  leg_left_4_joint:  &leg_4_gains {p:  5000, d: 20, i:  5, i_clamp:   25, torque_clamp: 300}
+  leg_left_5_joint:  &leg_5_gains {p:  5000, d: 20, i:  5, i_clamp:   14, torque_clamp: 160}
+  leg_left_6_joint:  &leg_6_gains {p:  5000, d: 20, i:  5, i_clamp:   9, torque_clamp: 100}
+  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_gazebo_alone.launch

@@ -18,13 +18,6 @@
 
   <env name="GAZEBO_MODEL_PATH" value="$(find talos_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
 
-  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
-  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
-  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
-  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
-  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
-  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.00 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" if="$(arg start_half_sitting)"/>
-
   <!-- start up world -->
   <include file="$(find pal_gazebo_worlds)/launch/pal_gazebo.launch">
     <arg name="world" value="$(arg world)"/>

launch/talos_gazebo_spawn_hs.launch

@@ -6,7 +6,7 @@
   <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
   <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
   <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
-  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" />
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) " />
 
   <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
   

launch/talos_gazebo_spawn_hs_wide.launch

 <?xml version="1.0" encoding="UTF-8"?>
 
 <launch>
-  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
-  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_leg_pose" default="-J leg_left_1_joint -0.001 -J leg_left_2_joint 0.062 -J leg_left_3_joint -0.4096 -J leg_left_4_joint 0.8568 -J leg_left_5_joint -0.4472 -J leg_left_6_joint -0.0600"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.002 -J leg_right_2_joint -0.0603 -J leg_right_3_joint -0.4094 -J leg_right_4_joint 0.8563 -J leg_right_5_joint -0.4469 -J leg_right_6_joint 0.0560"/>
   <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
   <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
   <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
@@ -10,7 +10,7 @@
 
   <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
   
-  <include file="$(find talos_data)/launch/talos_spawn.launch">
+  <include file="$(find talos_gazebo)/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_data)/config/pids.yaml"/>
+  <rosparam command="load" file="$(find talos_data)/config/pids_stiff.yaml"/>
 
   <!-- Spawn robot in Gazebo -->
   <node pkg="gazebo_ros" type="spawn_model" name="spawn_model"

launch/talos_spawn_stiff.launch

+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="robot"  default="full_v2"/>
+  <arg name="gzpose" default="-x 0 -y 0 -z 1.0 -R 0.0 -P 0.0 -Y 0.0"/>
+
+  <!-- PAL Hardware gazebo config -->
+  <include file="$(find talos_controller_configuration)/launch/selective_rosparam_loader.launch">
+    <arg name="robot" value="$(arg robot)" />
+    <arg name="prefix" value="$(find talos_hardware_gazebo)/config/sensors/" />
+  </include>
+
+
+  <!-- PID gains -->
+  <rosparam command="load" file="$(find talos_data)/config/pids_stiff.yaml"/>
+
+  <!-- Spawn robot in Gazebo -->
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_model"
+    args="-urdf -param robot_description $(arg gzpose) -model talos" />
+
+  <node ns="/rgbd/rgb/high_res" pkg="image_proc" type="image_proc" name="image_proc_high_res"/>
+  <node ns="/rgbd/rgb" pkg="image_proc" type="image_proc" name="image_proc"/>
+</launch>

scripts/start_talos_gazebo.py

@@ -26,7 +26,7 @@ roslaunch.configure_logging(uuid)
 
 cli_args = [talos_data_path+'/launch/talos_gazebo_alone.launch',
             'world:=empty_forced',
-            'enable_leg_passive:=true'
+            'enable_leg_passive:=false'
            ]
 roslaunch_args = cli_args[1:]
 roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]