Arguments for proper installation, timers to avoid blocking instructi…

…ons, better paths and arguments reading

CMakeLists.txt

@@ -80,3 +80,23 @@ PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
   INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
 ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+# Install scripts
+find_package(catkin REQUIRED)
+
+catkin_package()
+catkin_install_python(PROGRAMS
+                      script/sim_walk_torque.py
+                      script/run_test_utils.py
+                      script/sim_walk_vel.py
+                      script/test_ddp_sinu_effort.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque_online.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel_online.py
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+FOREACH(dir python traj_multicontact_api)
+  INSTALL(DIRECTORY ${dir}
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+ENDFOREACH(dir)

package.xml

@@ -11,5 +11,6 @@
   <author>Olivier Stasse</author>
   <maintainer email="guilhem.saurel@laas.fr">Guilhem Saurel</maintainer>
 
+  <buildtool_depend>catkin</buildtool_depend>
   <depend>sot-torque-control</depend>
 </package>

python/dynamic_graph/sot/torque_control/talos/balance_ctrl_sim_conf.py

@@ -34,8 +34,8 @@
   0.1, 0.1))
 
 kp_tau = 0.1 * np.array(
-(5.0, 5.0,  5.0,  5.0,  8.5,  8.5,
- 5.0, 5.0,  5.0,  5.0,  8.5,  8.5,
+(5.0, 5.0,  5.0,  5.0,  8.0,  8.0,
+ 5.0, 5.0,  5.0,  5.0,  8.0,  8.0,
  5.0, 5.0,
  5.0, 5.0,  5.0,  5.0,  5.0,  0.0, 0.0,
  0.0,

script/run_test_utils.py

@@ -5,7 +5,7 @@
 """
 from __future__ import print_function
 
-import rospy
+import rospy, time
 
 from std_srvs.srv import *
 from dynamic_graph_bridge_msgs.srv import *
@@ -26,7 +26,8 @@ def evalCommandClient(code):
     return eval(runCommandClient(code).result)
 
 def launch_script(code,title,description = ""):
-    raw_input(title+':   '+description)
+    #raw_input(title+':   '+description)
+    time.sleep(3)
     rospy.loginfo(title)
     rospy.loginfo(code)
     for line in code:
@@ -52,7 +53,8 @@ def run_test(appli):
         rospy.loginfo("Stack of Tasks launched")
 
         launch_script(initCode,'initialize SoT')
-        raw_input("Wait before starting the dynamic graph")
+        #raw_input("Wait before starting the dynamic graph")
+        rospy.sleep(6)
         runCommandStartDynamicGraph()
         print()
     except rospy.ServiceException, e:

script/sim_walk_torque.py

@@ -1,38 +1,46 @@
 #!/usr/bin/python
-import time, subprocess, os
+import time, subprocess, os, rospy
 from sys import argv
 from run_test_utils import runCommandClient, run_test
+import rospkg
 
 try:
     # Python 2
     input = raw_input  # noqa
 except NameError:
     pass
 
-folder = str(os.path.dirname(os.path.abspath(__file__)))
-folder = folder + "/../traj_multicontact_api/dat/"
+PKG_NAME='talos-torque-control'
+
+rospack = rospkg.RosPack()
+pkg_path = rospack.get_path(PKG_NAME)
+folder= pkg_path+ "/traj_multicontact_api/dat/"
 walk_type = "on_spot"
 pattern_generator = False
 
-if len(argv) == 2 and argv[1] == "on_spot":
+if len(argv) == 4 and argv[1] == "on_spot":
     print("Starting script with folder " + folder + " walking on spot.")
-elif len(argv) == 2 and argv[1] == "walk_20":
+elif len(argv) == 4 and argv[1] == "walk_20":
     print("Starting script with folder " + folder + " walking with 20cm step.")
     walk_type = "walk_20"
-elif len(argv) == 3 and argv[1] == "on_spot" and argv[2] == "pattern_generator":
+elif len(argv) == 5 and argv[1] == "on_spot" and argv[2] == "pattern_generator":
     pattern_generator = True
     print("Starting script with pattern_generator walking on spot.")
-elif len(argv) == 3 and argv[1] == "walk_20" and argv[2] == "pattern_generator":
+elif len(argv) == 5 and argv[1] == "walk_20" and argv[2] == "pattern_generator":
     pattern_generator = True
     walk_type = "walk_20"
     print("Starting script with pattern_generator walking with 20cm step.")
-elif len(argv) == 3 and argv[1] == "on_spot":
+elif len(argv) == 5 and argv[1] == "on_spot":
     folder = argv[2]
     print("Starting script with folder " + folder + " walking on spot.")
-elif len(argv) == 3 and argv[1] == "walk_20":
+elif len(argv) == 5 and argv[1] == "walk_20":
     folder = argv[2]
     print("Starting script with folder " + folder + " walking with 20cm step.")
     walk_type = "walk_20"
+# Default option: on_spot, without pattern generator
+elif len(argv) == 3:
+    print("Starting script with folder " + folder + " walking on spot.")
+
 else: 
     print("Usage: python sim_walk_torque.py walk_type:=[on_spot|walk_20] {pattern_generator|path_folder_of_the_reference_trajectories}")
     print("By giving only the walk_type the script starts using the default file trajectories")
@@ -42,13 +50,17 @@
     runCommandClient('folder = "' + folder + '"')
     runCommandClient('walk_type = "' + walk_type + '"')
     print("Starting script whith inverse_dyn_balance_controller main_sim_walk_torque.py")
-    run_test('../python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque.py')
+    run_test(pkg_path + '/../../lib/'+PKG_NAME+'/main_sim_walk_torque.py')
 else:
     runCommandClient('walk_type = "' + walk_type + '"')
     print("Starting script whith inverse_dyn_balance_controller main_sim_walk_torque_online.py")
-    run_test('../python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque_online.py')
+    run_test(pkg_path + '/../../lib/'+PKG_NAME+'/main_sim_walk_torque_online.py')
+
+#initializing node to read ros time
+rospy.init_node('test_torque_walking', anonymous=True)
 
-input("Waiting before writing the graph")
+#input("Waiting before writing the graph")
+time.sleep(5)
 runCommandClient("from dynamic_graph import writeGraph")
 
 print("WriteGraph in /tmp/sot_talos_tsid.dot")
@@ -57,32 +69,38 @@
 proc3 = subprocess.Popen(["dot", "-Tpdf", "/tmp/sot_talos_tsid_walk.dot", "-o", "/tmp/sot_talos_tsid_walk_torque.pdf"])
 
 if pattern_generator:
-    input("Waiting before setting gains")
+    #input("Waiting before setting gains")
+    time.sleep(3)
     print("Setting gains")
     runCommandClient("robot.inv_dyn.kp_feet.value = 6*(250,)")
-    runCommandClient("robot.inv_dyn.kp_com.value = 3*(400,)")
+    #runCommandClient("robot.inv_dyn.kp_com.value = 3*(400,)")
     runCommandClient("robot.inv_dyn.kd_feet.value = 6*(12,)")
-    runCommandClient("robot.inv_dyn.kd_com.value = 3*(12,)")
-    input("Waiting before playing trajectories")
+    #runCommandClient("robot.inv_dyn.kd_com.value = 3*(12,)")
+    #input("Waiting before playing trajectories")
+    time.sleep(2)
     runCommandClient('robot.triggerPG.sin.value = 1')
-    input("Wait before stopping the trajectory")
+    rospy.sleep(70) #arbitrary value
+    #input("Wait before stopping the trajectory")
     runCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)')
 else:
-    input("Waiting before setting gains")
+    #input("Waiting before setting gains")
+    time.sleep(3)
     print("Setting gains")
     runCommandClient("robot.inv_dyn.kp_feet.value = 6*(200,)")
-    runCommandClient("robot.inv_dyn.kp_com.value = 3*(250,)")
+    # runCommandClient("robot.inv_dyn.kp_com.value = 3*(250,)")
     runCommandClient("robot.inv_dyn.kd_feet.value = 6*(12,)")
-    runCommandClient("robot.inv_dyn.kd_com.value = 3*(12,)")
-    input("Waiting before playing trajectories")
+    # runCommandClient("robot.inv_dyn.kd_com.value = 3*(12,)")
+    #input("Waiting before playing trajectories")
+    time.sleep(2)
     print("Playing trajectories")
     runCommandClient("robot.traj_sync.turnOn()")
-    input("Waiting before stopping the trajectories")
+    rospy.sleep(64) #64 seconds is the time necessary for the on_spot trajectory (the longest one)
+    #input("Waiting before stopping the trajectories")
     print("Stop trajectories")
     runCommandClient("robot.traj_sync.turnOff()")
 
 time.sleep(2.0)
-input("Wait before going to halfSitting")
+#input("Wait before going to halfSitting")
 runCommandClient("go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0)")
 time.sleep(5.0)
 print("The robot is back in position!")

script/sim_walk_vel.py

@@ -1,38 +1,45 @@
 #!/usr/bin/python
-import time, subprocess, os
+import time, subprocess, os, rospy
 from sys import argv
 from run_test_utils import runCommandClient, run_test
+import rospkg
 
 try:
     # Python 2
     input = raw_input  # noqa
 except NameError:
     pass
 
-folder = str(os.path.dirname(os.path.abspath(__file__)))
-folder = folder + "/../traj_multicontact_api/dat/"
+PKG_NAME='talos-torque-control'
+
+rospack = rospkg.RosPack()
+pkg_path = rospack.get_path(PKG_NAME)
+folder= pkg_path+ "/traj_multicontact_api/dat/"
 walk_type = "on_spot_test"
 pattern_generator = False
 
-if len(argv) == 2 and argv[1] == "on_spot":
+if len(argv) == 4 and argv[1] == "on_spot":
     print("Starting script with folder " + folder + " walking on spot.")
-elif len(argv) == 2 and argv[1] == "walk_20":
+elif len(argv) == 4 and argv[1] == "walk_20":
     print("Starting script with folder " + folder + " walking with 20cm step.")
     walk_type = "walk_20_test"
-elif len(argv) == 3 and argv[1] == "on_spot" and argv[2] == "pattern_generator":
+elif len(argv) == 5 and argv[1] == "on_spot" and argv[2] == "pattern_generator":
     pattern_generator = True
     print("Starting script with pattern_generator walking on spot.")
-elif len(argv) == 3 and argv[1] == "walk_20" and argv[2] == "pattern_generator":
+elif len(argv) == 5 and argv[1] == "walk_20" and argv[2] == "pattern_generator":
     pattern_generator = True
     walk_type = "walk_20_test"
     print("Starting script with pattern_generator walking with 20cm step.")
-elif len(argv) == 3 and argv[1] == "on_spot":
+elif len(argv) == 5 and argv[1] == "on_spot":
     folder = argv[2]
     print("Starting script with folder " + folder + " walking on spot.")
-elif len(argv) == 3 and argv[1] == "walk_20":
+elif len(argv) == 5 and argv[1] == "walk_20":
     folder = argv[2]
     print("Starting script with folder " + folder + " walking with 20cm step.")
     walk_type = "walk_20"
+# Default option: on_spot_test, without pattern generator
+elif len(argv) == 3:
+    print("Starting script with folder " + folder + " walking on spot.")
 else: 
     print("Usage: python sim_walk_vel.py walk_type:=[on_spot|walk_20] {pattern_generator|path_folder_of_the_reference_trajectories}")
     print("By giving only the walk_type the script starts using the default file trajectories")
@@ -42,39 +49,48 @@
     runCommandClient('folder = "' + folder + '"')
     runCommandClient('walk_type = "' + walk_type + '"')
     print("Starting script whith inverse_dyn_balance_controller")
-    run_test('../python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel.py')
+    run_test(pkg_path+'/../../lib/'+PKG_NAME+'/main_sim_walk_vel.py')
 else:
     runCommandClient('walk_type = "' + walk_type + '"')
     print("Starting script whith inverse_dyn_balance_controller")
-    run_test('../python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel_online.py')
+    run_test(pkg_path+'/../../lib/'+PKG_NAME+'/main_sim_walk_vel_online.py')
+
+#initializing node to read ros time
+rospy.init_node('test_walking_vel', anonymous=True)
 
-input("Waiting before writing the graph")
+#input("Waiting before writing the graph")
+time.sleep(5)
 runCommandClient("from dynamic_graph import writeGraph")
 
 print("WriteGraph in /tmp/sot_talos_tsid.dot")
 runCommandClient("writeGraph('/tmp/sot_talos_tsid_walk.dot')")
 print("Convert graph to PDF in /tmp/sot_talos_tsid_walk.pdf")
 proc3 = subprocess.Popen(["dot", "-Tpdf", "/tmp/sot_talos_tsid_walk.dot", "-o", "/tmp/sot_talos_tsid_walk.pdf"])
 
-input("Waiting before setting gains")
+#input("Waiting before setting gains")
+time.sleep(3)
 print("Setting gains")
 # runCommandClient("robot.inv_dyn.kp_feet.value = 6*(500,)")
 # runCommandClient("robot.inv_dyn.kp_com.value = 3*(100,)")
 # runCommandClient("robot.inv_dyn.kd_feet.value = 6*(12,)")
 # runCommandClient("robot.inv_dyn.kd_com.value = 3*(7,)")
 
 if pattern_generator:
-    input("Waiting before playing trajectories")
+    #input("Waiting before playing trajectories")
+    time.sleep(2)
     runCommandClient('robot.triggerPG.sin.value = 1')
     runCommandClient('plug(robot.pg.leftfootref, robot.m2qLF.sin)')
     runCommandClient('plug(robot.pg.rightfootref, robot.m2qLF.sin)')
-    input("Wait before stopping the trajectory")
+    #input("Wait before stopping the trajectory")
+    rospy.sleep(70)
     runCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)')
 else:
-    input("Waiting before playing trajectories")
+    #input("Waiting before playing trajectories")
+    time.sleep(2)
     print("Playing trajectories")
     runCommandClient("robot.traj_sync.turnOn()")
-    input("Waiting before stopping the trajectories")
+    #input("Waiting before stopping the trajectories")
+    time.sleep(64)
     print("Stop trajectories")
     runCommandClient("robot.traj_sync.turnOff()")
 # input("Waiting before reading trajectories")
@@ -85,7 +101,7 @@
 # runCommandClient('robot.lf_traj_gen.playTrajectoryFile(folder + walk_type + "/leftFoot.dat")')
 
 time.sleep(2.0)
-input("Wait before going to halfSitting")
+#input("Wait before going to halfSitting")
 runCommandClient("go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0)")
 time.sleep(5.0)
 print("The robot is back in position!")