diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2bc40d3..96eb00d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,6 +59,8 @@ catkin_install_python(PROGRAMS
scripts/start_talos_gazebo_kine.py
scripts/start_sot_talos_balance.py
scripts/start_sot_online_walking.py
+ scripts/start_sot_ouster_walking.py
+ scripts/start_torque_control.py
scripts/test_kine.py
scripts/test_sot_talos_balance.py
scripts/test_online_walking.py
@@ -77,5 +79,12 @@ if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest(tests/test_kine.test)
add_rostest(tests/test_sot_talos_balance.test)
- add_rostest(tests/test_online_walking.test)
+ add_rostest(tests/test_online_walking.test)
+ add_rostest(tests/test_ouster_walking.test)
+ add_rostest(tests/test_torque_control_on_spot.test) #add_rostest(tests/test_torque_control.test ARGS arg1:=true arg2:=false)
+ add_rostest(tests/test_torque_control_walk_20.test)
+ add_rostest(tests/test_torque_control_on_spot_pg.test)
+ add_rostest(tests/test_torque_control_walk_20_pg.test)
+
+
endif()
diff --git a/launch/talos_spawn_hs_ouster.launch b/launch/talos_spawn_hs_ouster.launch
new file mode 100644
index 0000000..35fed83
--- /dev/null
+++ b/launch/talos_spawn_hs_ouster.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/package.xml b/package.xml
index 1c33dd9..d1187d4 100644
--- a/package.xml
+++ b/package.xml
@@ -25,5 +25,7 @@
talos_gazebo
roscontrol_sot_talos
dynamic_graph_bridge_msgs
-
+ sot-pattern-generator
+ talos_bauzil
+ talos-torque-control
diff --git a/scripts/start_sot_online_walking.py b/scripts/start_sot_online_walking.py
index b2c123b..c4b2fd4 100755
--- a/scripts/start_sot_online_walking.py
+++ b/scripts/start_sot_online_walking.py
@@ -143,7 +143,10 @@ def runTest(self):
print("Stop roscore")
roscore.terminate()
- r.sleep()
+ try:
+ r.sleep()
+ except rospy.ROSInterruptException:
+ rospy.logwarn("Exiting test")
if __name__ == '__main__':
import rosunit
diff --git a/scripts/start_sot_ouster_walking.py b/scripts/start_sot_ouster_walking.py
new file mode 100755
index 0000000..aa0f032
--- /dev/null
+++ b/scripts/start_sot_ouster_walking.py
@@ -0,0 +1,202 @@
+#!/usr/bin/env python
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+# from package robotpkg-talos-data
+# Modified by H. Lefevre 02/07/2020
+# LAAS, CNRS
+
+import os
+import rospy
+import time
+import roslaunch
+import rospkg
+import unittest
+import math
+
+from gazebo_msgs.srv import *
+
+from std_srvs.srv import Empty
+
+PKG_NAME='talos_integration_tests'
+
+class TestSoTTalos(unittest.TestCase):
+
+ def validation_through_gazebo(self):
+ gzGetModelPropReq = rospy.ServiceProxy('/gazebo/get_model_state',
+ GetModelState)
+ gzGetModelPropResp = gzGetModelPropReq(model_name='talos')
+ f=open("/tmp/output.dat","w+")
+ f.write("x:"+str(gzGetModelPropResp.pose.position.x)+"\n")
+ f.write("y:"+str(gzGetModelPropResp.pose.position.y)+"\n")
+ f.write("z:"+str(gzGetModelPropResp.pose.position.z)+"\n")
+ # dx depends on the timing of the simulation
+ # which can be different from one computer to another.
+ # Therefore check only dy and dz.
+ dx=0.0;
+ dy=gzGetModelPropResp.pose.position.y--1.51
+ dz=gzGetModelPropResp.pose.position.z-0.997
+ ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
+ f.write("dist:"+str(ldistance))
+ f.close()
+ if ldistance<0.1:
+ self.assertTrue(True,msg="Converged to the desired position")
+ else:
+ self.assertFalse(True,
+ msg="Did not converge to the desired position")
+
+ def runTest(self):
+ # Start roscore
+ import subprocess
+ roscore = subprocess.Popen('roscore')
+ time.sleep(2)
+
+ # Get the path to talos_data
+ arospack = rospkg.RosPack()
+ talos_data_path = arospack.get_path('talos_data')
+ talos_bauzil_path = arospack.get_path('talos_bauzil')
+ talos_test_path = arospack.get_path('talos_integration_tests')
+
+ # 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_bauzil_path+'/launch/script_walking_bauzil.launch',
+ 'world_name:=bauzil_skins',
+ #'robot:=full_v2',
+ 'robot:=full_v2_ouster',
+ '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_test_path+'/launch/talos_spawn_hs_ouster.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")
+
+ time.sleep(3)
+
+ # Launch test
+ pkg_name='talos_integration_tests'
+ executable='test_online_walking.py'
+ node_name='test_online_walking_py'
+ test_sot_ouster_walking_node = roslaunch.core.\
+ Node(pkg_name, executable,name=node_name)
+
+ launch_test_sot_ouster_walking=roslaunch.scriptapi.ROSLaunch()
+ launch_test_sot_ouster_walking.start()
+
+ test_sot_ouster_walking_process = launch_test_sot_ouster_walking.\
+ launch(test_sot_ouster_walking_node)
+
+ time.sleep(3)
+
+ # Publish odometry
+ launch_odom = roslaunch.parent.ROSLaunchParent(uuid,
+ [talos_bauzil_path+'/launch/talos_odometry.launch'])
+ launch_odom.start()
+ rospy.loginfo("Talos odometry started")
+
+ time.sleep(2)
+
+ # Start mapping
+ aicp_path = arospack.get_path('aicp_ros')
+ cli_args = [aicp_path+'/launch/aicp_mapping.launch',
+ 'pose_odom:=/odom_aicp',
+ 'lidar_channel:=/os1_cloud_node/points',
+ 'inertial_frame:=/world',
+ 'fixed_frame:=/world'
+ ]
+
+ roslaunch_args = cli_args[1:]
+ roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]
+
+ launch_aicp = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)
+ launch_aicp.start()
+ rospy.loginfo("aicp_mapping started")
+
+
+
+ rospy.sleep(3)
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ # Test if sot_online_walking is finished or not
+ if not test_sot_ouster_walking_process.is_alive():
+
+ self.validation_through_gazebo()
+
+
+ # If it is finished then find exit status.
+ if test_sot_ouster_walking_process.exit_code != 0:
+ exit_status = "test_ouster_walking failed"
+ self.assertFalse(True,exit_status)
+ else:
+ exit_status="None"
+
+ print("Stopping SoT")
+ launch_roscontrol_sot_talos.shutdown()
+ # print("Shutting down spawners")
+ # launch_gazebo_spawn_hs.shutdown()
+ # launch_bringup.shutdown()
+ print("Stopping Odometry")
+ launch_odom.shutdown()
+ print("Stopping Gazebo")
+ launch_gazebo_alone.shutdown()
+ print("Stopping Mapping")
+ launch_aicp.shutdown()
+
+
+ rospy.signal_shutdown(exit_status)
+
+ # Terminate the roscore subprocess
+ print("Stop roscore")
+ roscore.terminate()
+
+
+ try:
+ r.sleep()
+ except rospy.ROSInterruptException:
+ rospy.logwarn("Exiting test")
+
+
+
+if __name__ == '__main__':
+ import rosunit
+ rosunit.unitrun(PKG_NAME,'test_online_walking',TestSoTTalos)
+
diff --git a/scripts/start_sot_talos_balance.py b/scripts/start_sot_talos_balance.py
index bd3394c..cb88ed6 100755
--- a/scripts/start_sot_talos_balance.py
+++ b/scripts/start_sot_talos_balance.py
@@ -32,7 +32,7 @@ def validation_through_gazebo(self):
ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
f.write("dist:"+str(ldistance))
f.close()
- if ldistance<0.009:
+ if ldistance<0.04:
self.assertTrue(True,msg="Converged to the desired position")
else:
self.assertFalse(True,
@@ -138,7 +138,10 @@ def runTest(self):
print("Stop roscore")
roscore.terminate()
- r.sleep()
+ try:
+ r.sleep()
+ except rospy.ROSInterruptException:
+ rospy.logwarn("Exiting test")
if __name__ == '__main__':
import rosunit
diff --git a/scripts/start_talos_gazebo_kine.py b/scripts/start_talos_gazebo_kine.py
index add2cbb..c1212f4 100755
--- a/scripts/start_talos_gazebo_kine.py
+++ b/scripts/start_talos_gazebo_kine.py
@@ -133,7 +133,10 @@ def runTest(self):
print("Stop roscore")
roscore.terminate()
- r.sleep()
+ try:
+ r.sleep()
+ except rospy.ROSInterruptException:
+ rospy.logwarn("Exiting test")
if __name__ == '__main__':
diff --git a/scripts/start_torque_control.py b/scripts/start_torque_control.py
new file mode 100755
index 0000000..024a4f4
--- /dev/null
+++ b/scripts/start_torque_control.py
@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+# H. Lefevre 05/08/2020
+# LAAS, CNRS
+
+import os
+import rospy
+import time
+import roslaunch
+import rospkg
+import unittest
+import math
+from sys import argv
+
+from gazebo_msgs.srv import *
+
+from std_srvs.srv import Empty
+
+PKG_NAME='talos_integration_tests'
+
+class TestSoTTalos(unittest.TestCase):
+
+ def validation_through_gazebo(self):
+ gzGetModelPropReq = rospy.ServiceProxy('/gazebo/get_model_state',
+ GetModelState)
+ gzGetModelPropResp = gzGetModelPropReq(model_name='talos')
+ f=open("/tmp/output.dat","w+")
+ f.write("x:"+str(gzGetModelPropResp.pose.position.x)+"\n")
+ f.write("y:"+str(gzGetModelPropResp.pose.position.y)+"\n")
+ f.write("z:"+str(gzGetModelPropResp.pose.position.z)+"\n")
+ # dx depends on the timing of the simulation
+ # which can be different from one computer to another.
+ # Therefore check only dy and dz.
+ dx=0.0;
+ dy=gzGetModelPropResp.pose.position.y-0.0038
+ dz=gzGetModelPropResp.pose.position.z-1.00152
+ ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
+ f.write("dist:"+str(ldistance))
+ f.close()
+ if ldistance<0.07:
+ self.assertTrue(True,msg="Converged to the desired position")
+ else:
+ self.assertFalse(True,
+ msg="Did not converge to the desired position")
+
+ def runTest(self):
+ # 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,
+ # [ros_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_effort.launch'])
+ launch_roscontrol_sot_talos.start()
+ rospy.loginfo("roscontrol_sot_talos started")
+
+ time.sleep(5)
+ pkg_name='talos-torque-control'
+
+ if len(argv)==7 and argv[1]=='vel' and argv[3]=='pattern_generator':
+ #starting sim_walk_vel with type of walk and pg
+ mode=str(argv[2]+' '+argv[3])
+ executable='integ_sim_walk_vel.py'
+ node_name='sim_walk_vel_py'
+ elif len(argv)==6 and argv[1]=='vel':
+ #starting sim_walk_vel without pg
+ mode=str(argv[2])
+ executable='integ_sim_walk_vel.py'
+ node_name='sim_walk_vel_py'
+ elif len(argv)==6 and argv[2]=='pattern_generator':
+ #pg argument but length of 6 means no vel argument --> sim_walk_torque
+ mode=str(argv[1]+' '+argv[2])
+ executable='integ_sim_walk_torque.py'
+ node_name='sim_walk_torque_py'
+ elif len(argv)==5:
+ #walk type only will mean walk_torque
+ mode=str(argv[1])
+ executable='integ_sim_walk_torque.py'
+ node_name='sim_walk_torque_py'
+ else: raise ValueError(str(len(argv))+' '+str(argv[0])+' '+str(argv[1])+' '+str(argv[2])+' '+str(argv[3])+' '+str(argv[4])+' '+str(argv[5]))
+
+ sim_walk_torque_node = roslaunch.core.\
+ Node(pkg_name, executable,name=node_name,args=mode)
+
+ launch_sim_walk_torque=roslaunch.scriptapi.ROSLaunch()
+ launch_sim_walk_torque.start()
+
+ sim_walk_torque_process = launch_sim_walk_torque.\
+ launch(sim_walk_torque_node)
+
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ # Test if sim_walk_torque is finished or not
+ if not sim_walk_torque_process.is_alive():
+
+ self.validation_through_gazebo()
+
+
+ # If it is finished then find exit status.
+ if sim_walk_torque_process.exit_code != 0:
+ exit_status = "sim_walk_torque failed"
+ self.assertFalse(True,exit_status)
+ else:
+ exit_status=None
+
+ print("Stopping SoT")
+ launch_roscontrol_sot_talos.shutdown()
+ print("Stopping Gazebo")
+ launch_gazebo_alone.shutdown()
+
+ rospy.signal_shutdown(exit_status)
+
+ # Terminate the roscore subprocess
+ print("Stop roscore")
+ roscore.terminate()
+
+ try:
+ r.sleep()
+ except rospy.ROSInterruptException:
+ rospy.logwarn("Exiting test")
+
+if __name__ == '__main__':
+ import rosunit
+ rosunit.unitrun(PKG_NAME,'sim_walk_torque',TestSoTTalos)
diff --git a/scripts/test_kine.py b/scripts/test_kine.py
index c879ecc..42fce44 100755
--- a/scripts/test_kine.py
+++ b/scripts/test_kine.py
@@ -1,4 +1,4 @@
-#! /usr/bin/env python
+#!/usr/bin/env python
import sys
import rospy
import time
@@ -21,6 +21,8 @@ def handleRunCommandClient(code):
PKG_NAME='talos_integration_tests'
+rospy.init_node('test_kine', anonymous=True)
+
def runTest():
# Waiting for services
try:
@@ -56,7 +58,7 @@ def runTest():
handleRunCommandClient("gotoNd(robot.taskRH,target,'111',(4.9,0.9,0.01,0.9))")
handleRunCommandClient("robot.sot.push(robot.taskRH.task.name)")
- time.sleep(10)
+ rospy.sleep(10)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
diff --git a/scripts/test_online_walking.py b/scripts/test_online_walking.py
old mode 100644
new mode 100755
index e9625f6..be7312b
--- a/scripts/test_online_walking.py
+++ b/scripts/test_online_walking.py
@@ -1,4 +1,4 @@
-#! /usr/bin/env python
+#!/usr/bin/env python
import sys
import rospy
import rospkg
@@ -68,9 +68,11 @@ def wait_for_dynamic_graph():
rospy.loginfo("Stack of Tasks launched")
+#initializing node to read ros time
+rospy.init_node('test_online_walking', anonymous=True)
#run_test(appli_file_name, verbosity=1,interactive=False)
-time.sleep(5)
+rospy.sleep(3)
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
@@ -91,13 +93,18 @@ def wait_for_dynamic_graph():
print('Executing the trajectory')
handleRunCommandClient('robot.triggerPG.sin.value = 1')
-time.sleep(4)
-handleRunCommandClient('robot.pg.velocitydes.value=(0.2,0.0,0.0)')
-time.sleep(7)
-handleRunCommandClient('robot.pg.velocitydes.value=(0.3,0.0,0.0)')
-time.sleep(9)
-handleRunCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)')
-time.sleep(9)
+
+rospy.sleep(1)
+
+
+handleRunCommandClient('robot.pg.velocitydes.value=(0.5,0.0,0.0)')
+rospy.sleep(5)
+handleRunCommandClient('robot.pg.velocitydes.value=(0.7,0.0,0.0)')
+rospy.sleep(4)
+handleRunCommandClient('robot.pg.velocitydes.value=(0.9,0.0,0.0)')
+rospy.sleep(10)
+handleRunCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)')
+rospy.sleep(3)
handleRunCommandClient('from sot_talos_balance.create_entities_utils import dump_tracer')
handleRunCommandClient('dump_tracer(robot.tracer)')
diff --git a/scripts/test_sot_talos_balance.py b/scripts/test_sot_talos_balance.py
index 3ab1555..d3b368d 100755
--- a/scripts/test_sot_talos_balance.py
+++ b/scripts/test_sot_talos_balance.py
@@ -1,4 +1,4 @@
-#! /usr/bin/env python
+#!/usr/bin/env python
import sys
import rospy
import time
@@ -19,6 +19,8 @@ def handleRunCommandClient(code):
PKG_NAME='talos_integration_tests'
+rospy.init_node('test_balance_walking', anonymous=True)
+
'''Test CoM admittance control as described in paper, with pre-loaded movements'''
from sot_talos_balance.utils.run_test_utils import \
run_ft_calibration, run_test, runCommandClient
@@ -26,7 +28,7 @@ def handleRunCommandClient(code):
# get the file path for rospy_tutorials
appli_file_name = join(dirname(abspath(__file__)), 'appli_dcmZmpControl_file.py')
-time.sleep(2)
+rospy.sleep(2)
rospy.loginfo("Stack of Tasks launched")
test_folder = 'TestKajita2003StraightWalking64/20cm'
@@ -40,12 +42,12 @@ def handleRunCommandClient(code):
handleRunCommandClient('from talos_integration_tests.appli_dcmZmpControl_file import init_sot_talos_balance')
handleRunCommandClient('init_sot_talos_balance(robot,\''+test_folder+'\')')
-time.sleep(5)
+rospy.sleep(3)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
Empty)
runCommandStartDynamicGraph()
-time.sleep(5)
+rospy.sleep(3)
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
handleRunCommandClient('from dynamic_graph import plug')
@@ -59,6 +61,6 @@ def handleRunCommandClient(code):
handleRunCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
print('Executing the trajectory')
-time.sleep(1)
+rospy.sleep(1)
handleRunCommandClient('robot.triggerTrajGen.sin.value = 1')
-time.sleep(25)
+rospy.sleep(20)
diff --git a/tests/test_online_walking.test b/tests/test_online_walking.test
index b07f205..a6f4c17 100644
--- a/tests/test_online_walking.test
+++ b/tests/test_online_walking.test
@@ -1,7 +1,7 @@
-
+
diff --git a/tests/test_ouster_walking.test b/tests/test_ouster_walking.test
new file mode 100644
index 0000000..ef82eaa
--- /dev/null
+++ b/tests/test_ouster_walking.test
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
diff --git a/tests/test_sot_talos_balance.test b/tests/test_sot_talos_balance.test
index 1423050..4ec636b 100644
--- a/tests/test_sot_talos_balance.test
+++ b/tests/test_sot_talos_balance.test
@@ -1,7 +1,7 @@
-
+
diff --git a/tests/test_torque_control_on_spot.test b/tests/test_torque_control_on_spot.test
new file mode 100644
index 0000000..19770dc
--- /dev/null
+++ b/tests/test_torque_control_on_spot.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_control_on_spot_pg.test b/tests/test_torque_control_on_spot_pg.test
new file mode 100644
index 0000000..64b1dce
--- /dev/null
+++ b/tests/test_torque_control_on_spot_pg.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_control_walk_20.test b/tests/test_torque_control_walk_20.test
new file mode 100644
index 0000000..e416ac3
--- /dev/null
+++ b/tests/test_torque_control_walk_20.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_control_walk_20_pg.test b/tests/test_torque_control_walk_20_pg.test
new file mode 100644
index 0000000..e9de494
--- /dev/null
+++ b/tests/test_torque_control_walk_20_pg.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_walk_vel_on_spot.test b/tests/test_torque_walk_vel_on_spot.test
new file mode 100644
index 0000000..eb1d316
--- /dev/null
+++ b/tests/test_torque_walk_vel_on_spot.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_walk_vel_on_spot_pg.test b/tests/test_torque_walk_vel_on_spot_pg.test
new file mode 100644
index 0000000..784ae8b
--- /dev/null
+++ b/tests/test_torque_walk_vel_on_spot_pg.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_walk_vel_walk_20.test b/tests/test_torque_walk_vel_walk_20.test
new file mode 100644
index 0000000..f9fab1a
--- /dev/null
+++ b/tests/test_torque_walk_vel_walk_20.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tests/test_torque_walk_vel_walk_20_pg.test b/tests/test_torque_walk_vel_walk_20_pg.test
new file mode 100644
index 0000000..6e562a4
--- /dev/null
+++ b/tests/test_torque_walk_vel_walk_20_pg.test
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file