From 2211e13a6860fa5d294c36a940cc6ef8a69cdf3f Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Thu, 30 Jan 2020 02:09:23 +0100
Subject: [PATCH] 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 | 54 ++++++++++++++++++++++++
launch/talos_bringup.launch | 13 ++++++
launch/talos_gazebo_alone.launch | 10 -----
launch/talos_gazebo_spawn_hs.launch | 2 +-
launch/talos_gazebo_spawn_hs_wide.launch | 2 +-
launch/talos_spawn.launch | 2 +-
scripts/start_talos_gazebo.py | 23 +++++++---
7 files changed, 87 insertions(+), 19 deletions(-)
create mode 100644 config/pids.yaml
create mode 100644 launch/talos_bringup.launch
diff --git a/config/pids.yaml b/config/pids.yaml
new file mode 100644
index 0000000..15c6cf4
--- /dev/null
+++ b/config/pids.yaml
@@ -0,0 +1,54 @@
+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
diff --git a/launch/talos_bringup.launch b/launch/talos_bringup.launch
new file mode 100644
index 0000000..c66d427
--- /dev/null
+++ b/launch/talos_bringup.launch
@@ -0,0 +1,13 @@
+<?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>
diff --git a/launch/talos_gazebo_alone.launch b/launch/talos_gazebo_alone.launch
index b3dff9d..6fc74d8 100644
--- a/launch/talos_gazebo_alone.launch
+++ b/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>
diff --git a/launch/talos_gazebo_spawn_hs.launch b/launch/talos_gazebo_spawn_hs.launch
index 36cecbc..3d3bb85 100644
--- a/launch/talos_gazebo_spawn_hs.launch
+++ b/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>
diff --git a/launch/talos_gazebo_spawn_hs_wide.launch b/launch/talos_gazebo_spawn_hs_wide.launch
index 36cecbc..3d3bb85 100644
--- a/launch/talos_gazebo_spawn_hs_wide.launch
+++ b/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>
diff --git a/launch/talos_spawn.launch b/launch/talos_spawn.launch
index aad59da..bf28b5d 100644
--- a/launch/talos_spawn.launch
+++ b/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"
diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py
index 101e6b2..452d194 100755
--- a/scripts/start_talos_gazebo.py
+++ b/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")
--
GitLab