diff --git a/config/pids_spring.yaml b/config/pids_spring.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f126e5ccfb6843d1644deaf2f1ff6058cf09ef03
--- /dev/null
+++ b/config/pids_spring.yaml
@@ -0,0 +1,54 @@
+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
diff --git a/config/pids.yaml b/config/pids_stiff.yaml
similarity index 100%
rename from config/pids.yaml
rename to config/pids_stiff.yaml
diff --git a/launch/talos_gazebo_alone.launch b/launch/talos_gazebo_alone.launch
index 6fc74d864c16ab45ac99eadcd24e70bf610d4572..5f2fb7ceafd90854ebb611fcc38e082200a2ae15 100644
--- a/launch/talos_gazebo_alone.launch
+++ b/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)"/>
diff --git a/launch/talos_gazebo_spawn_hs.launch b/launch/talos_gazebo_spawn_hs.launch
index 3d3bb8503078595946e8345bb29afdcf3cfff7e2..2b917ab976bd13258daf5debef43a39b548112c5 100644
--- a/launch/talos_gazebo_spawn_hs.launch
+++ b/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 -->
diff --git a/launch/talos_gazebo_spawn_hs_wide.launch b/launch/talos_gazebo_spawn_hs_wide.launch
index 3d3bb8503078595946e8345bb29afdcf3cfff7e2..54238b116e7b337d89b54eaaeda638bc87821d53 100644
--- a/launch/talos_gazebo_spawn_hs_wide.launch
+++ b/launch/talos_gazebo_spawn_hs_wide.launch
@@ -1,8 +1,8 @@
<?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>
diff --git a/launch/talos_spawn.launch b/launch/talos_spawn.launch
index bf28b5dffcf479b42473f5dce37357184e83ea02..6eb48e0d705099502458635d58ad6f48fbaa26e0 100644
--- a/launch/talos_spawn.launch
+++ b/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"
diff --git a/launch/talos_spawn_stiff.launch b/launch/talos_spawn_stiff.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6eb48e0d705099502458635d58ad6f48fbaa26e0
--- /dev/null
+++ b/launch/talos_spawn_stiff.launch
@@ -0,0 +1,23 @@
+<?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>
diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py
index 452d194215298686613edca3b162790356a3d542..69a4d99a9bf49bae27c67461254fd69231f75ada 100755
--- a/scripts/start_talos_gazebo.py
+++ b/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)]