From d3973b8ce1240d89c98e9572fc6c13a6d5dcfbb8 Mon Sep 17 00:00:00 2001 From: Hugo Date: Tue, 25 Aug 2020 13:30:12 +0200 Subject: [PATCH] Arguments for proper installation, timers to avoid blocking instructions, better paths and arguments reading --- CMakeLists.txt | 20 ++++++ package.xml | 1 + .../talos/balance_ctrl_sim_conf.py | 4 +- script/run_test_utils.py | 8 ++- script/sim_walk_torque.py | 64 ++++++++++++------- script/sim_walk_vel.py | 52 +++++++++------ 6 files changed, 103 insertions(+), 46 deletions(-) mode change 100644 => 100755 script/sim_walk_torque.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f41aa98..5ba5864 100644 --- a/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/package.xml b/package.xml index d61c6a1..6ef1a48 100644 --- a/package.xml +++ b/package.xml @@ -11,5 +11,6 @@ Olivier Stasse Guilhem Saurel + catkin sot-torque-control diff --git a/python/dynamic_graph/sot/torque_control/talos/balance_ctrl_sim_conf.py b/python/dynamic_graph/sot/torque_control/talos/balance_ctrl_sim_conf.py index 48d5612..3dc280e 100644 --- a/python/dynamic_graph/sot/torque_control/talos/balance_ctrl_sim_conf.py +++ b/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, diff --git a/script/run_test_utils.py b/script/run_test_utils.py index 7f525a5..5165287 100755 --- a/script/run_test_utils.py +++ b/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: diff --git a/script/sim_walk_torque.py b/script/sim_walk_torque.py old mode 100644 new mode 100755 index f7da821..15b5e4b --- a/script/sim_walk_torque.py +++ b/script/sim_walk_torque.py @@ -1,7 +1,8 @@ #!/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 @@ -9,30 +10,37 @@ 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!") diff --git a/script/sim_walk_vel.py b/script/sim_walk_vel.py index 14b1231..bef5eda 100644 --- a/script/sim_walk_vel.py +++ b/script/sim_walk_vel.py @@ -1,7 +1,8 @@ #!/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 @@ -9,30 +10,36 @@ 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,13 +49,17 @@ 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") @@ -56,7 +67,8 @@ 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,)") @@ -64,17 +76,21 @@ # 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!")