roscontrol_sot_talos/scripts/start_talos_gazebo.py

+#!/usr/bin/env python
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+
+import rospy
+import time
+import roslaunch
+import rospkg
+
+from std_srvs.srv import Empty
+
+# Start roscore
+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 = [
+    talos_data_path + "/launch/talos_gazebo_alone.launch",
+    "world:=empty_forced",
+    "enable_leg_passive:=false",
+]
+roslaunch_args = cli_args[1:]
+roslaunch_file = [
+    (roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)
+]
+
+launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)
+launch_gazebo_alone.start()
+rospy.loginfo("talos_gazebo_alone started")
+
+rospy.wait_for_service("/gazebo/pause_physics")
+gazebo_pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
+gazebo_pause_physics()
+
+time.sleep(5)
+# Spawn talos model in gazebo
+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")
+
+rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
+time.sleep(5)
+gazebo_unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
+gazebo_unpause_physics()
+
+# Start roscontrol
+launch_bringup = roslaunch.parent.ROSLaunchParent(
+    uuid, [talos_data_path + "/launch/talos_bringup.launch"]
+)
+launch_bringup.start()
+rospy.loginfo("talos_bringup started")
+
+# Start sot
+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")
+
+rospy.spin()

roscontrol_sot_talos/scripts/start_talos_gazebo_16_04.py

+#!/usr/bin/env python2.7
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+
+# This file is a temporary fix for the ubuntu version 16.04 of the script
+# start_talos_gazebo.py.
+# The path to talos_data cannot be retreive using the rospkg.RosPack() and
+# get_path('talos_data') in 16.04.
+# This should be investigated (see issue #4 "Hardcoded talos_data path for 16.04 in
+# script start_talos_gazebo_16_04.py").
+
+import rospy
+import time
+import roslaunch
+from std_srvs.srv import Empty
+
+# Start roscore
+import subprocess
+
+roscore = subprocess.Popen("roscore")
+time.sleep(1)
+
+# Start talos_gazebo
+rospy.init_node("starting_talos_gazebo", anonymous=True)
+uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+roslaunch.configure_logging(uuid)
+
+launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(
+    uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch"]
+)
+launch_gazebo_alone.start()
+rospy.loginfo("talos_gazebo_alone started")
+
+rospy.wait_for_service("/gazebo/pause_physics")
+gazebo_pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
+gazebo_pause_physics()
+
+time.sleep(3)
+# 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.start()
+rospy.loginfo("talos_gazebo_spawn_hs started")
+
+rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
+time.sleep(3)
+gazebo_unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
+gazebo_unpause_physics()
+
+# Start roscontrol
+launch_bringup = roslaunch.parent.ROSLaunchParent(
+    uuid, ["/opt/openrobots/share/talos_data/launch/talos_bringup.launch"]
+)
+launch_bringup.start()
+rospy.loginfo("talos_bringup started")
+
+# # Start sot
+# Start sot in another terminal with "roslaunch roscontrol_sot_talos
+# sot_talos_controller_gazebo.launch".
+# in order to have the logs saved. Otherwise the data are not correctly dumped when the
+# process is killed.
+
+
+rospy.spin()

setup.cfg

+[flake8]
+exclude = cmake
+max-line-length = 88
+extend-ignore = E203

sot_pyrene_bringup/CMakeLists.txt

@@ -5,35 +5,19 @@ project(sot_pyrene_bringup)
 find_package(catkin REQUIRED)
 catkin_package()
 
-#set(PROJECT_NAME sot_talos_bringup)
-#set(PROJECT_DESCRIPTION ROS package for Stack of Tasks on Talos)
-#set(PROJECT_URL "")
-
-#setup_project()
-
-INSTALL(
-  FILES 
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
-)
-
-INSTALL(
-  FILES
-  launch/geometric_simu.launch
-  launch/geometric_simu_context.launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
-
-INSTALL(
-  FILES
-  sot/pyrene.yaml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sot
-)
-
-INSTALL(
-  FILES
-  package.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
-)
-
-#setup_project_finalize()
+# set(PROJECT_NAME sot_talos_bringup) set(PROJECT_DESCRIPTION ROS package for
+# Stack of Tasks on Talos) set(PROJECT_URL "")
 
+# setup_project()
+
+install(FILES DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
+
+install(FILES launch/geometric_simu.launch launch/geometric_simu_context.launch
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+
+install(FILES sot/pyrene.yaml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sot)
+
+install(FILES package.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/)
+
+# setup_project_finalize()

sot_pyrene_bringup/launch/geometric_simu.launch

@@ -4,22 +4,22 @@
 <launch>
   <!-- Which robot are we controlling ? -->
   <arg name="robot" default="pyrene" />
-  <arg name="libsot" default="libsot-pyrene-controller.so" />   
+  <arg name="libsot" default="libsot-pyrene-controller.so" />
 
   <arg name="sot-launch-prefix" default="" />
-  
+
   <include file="$(find sot_pyrene_bringup)/launch/geometric_simu_context.launch" >
-    <arg name="robot" value="$(arg robot)" />       
+    <arg name="robot" value="$(arg robot)" />
     <arg name="libsot" value="$(arg libsot)" />
   </include>
 
   <!-- Load Stack of Tasks. -->
-  <node machine="geometric_simu_machine" 
-        name="node_stack_of_tasks" 
+  <node machine="geometric_simu_machine"
+        name="node_stack_of_tasks"
 	pkg="dynamic_graph_bridge"
 	type="geometric_simu"
         args=" --input-file $(arg libsot)"
-	launch-prefix="$(arg sot-launch-prefix)"
+        launch-prefix="$(arg sot-launch-prefix)"
 	respawn="true">
     <param name="/sot/dg/geometric_simu" value="" />
   </node>

sot_pyrene_bringup/launch/geometric_simu_context.launch

@@ -4,22 +4,21 @@
 <launch>
   <!-- Which robot are we controlling ? -->
   <arg name="robot" default="pyrene" />
-  <arg name="libsot" default="libsot-pyrene-controller.so" />   
+  <arg name="libsot" default="libsot-pyrene-controller.so" />
 
   <!-- BTW we are in simulation ? -->
 
    <!-- Load robot model. -->
   <param name="robot_description"
-	 textfile="$(find talos_data)/urdf/talos_reduced.urdf" />
+	 textfile="$(find talos_data)/urdf/pyrene.urdf" />
 
 
    <!-- Load robot sot params. -->
   <rosparam command="load"
 	 file="$(find sot_pyrene_bringup)/sot/pyrene.yaml" />
-  
+
   <machine name="geometric_simu_machine"
-           address="localhost">
-  </machine >
+    address="localhost" />
 
   <!--
      Read joint_states topic and publish link positions to tf.
@@ -30,7 +29,7 @@
     -->
   <node name="robot_state_publisher"
 	pkg="robot_state_publisher"
-	type="state_publisher"
+	type="robot_state_publisher"
 	respawn="true">
     <param name="tf_prefix" value="" />
   </node>

sot_pyrene_bringup/package.xml

 <package>
   <name>sot_pyrene_bringup</name>
-  <version>0.0.8</version>
+  <version>0.3.1</version>
   <description>ROS package for Stack of Tasks on Pyrene</description>
 
   <maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>

sot_pyrene_bringup/sot/pyrene.yaml

 sot:
-  state_vector_map: [leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint, 
-    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint, 
+  state_vector_map: [leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
     torso_1_joint, torso_2_joint,
     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, gripper_left_joint,
     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, gripper_right_joint,
@@ -18,4 +18,10 @@ sot:
     gripper_right_inner_single_joint: -gripper_right_joint,
     gripper_right_fingertip_3_joint: -gripper_right_joint}
 sot_controller:
-  dt: 0.001
\ No newline at end of file
+  joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
+    torso_1_joint,torso_2_joint,
+    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, gripper_left_joint,
+    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, gripper_right_joint, head_1_joint, head_2_joint
+  ]
+  dt: 0.001

talos_metapkg_ros_control_sot/package.xml

 <package>
   <name>talos_metapkg_ros_control_sot</name>
-  <version>0.0.8</version>
+  <version>0.3.1</version>
   <description>A set of packages that include sot-wrapper and talos sot params .</description>
 
   <maintainer email="olivier.stasse@laas.fr">Olivier Stasse</maintainer>