Skip to content

GitLab

Commit d26f4fb8 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format] yapf

parent 7f93d788
Hide whitespace changes
Inline Side-by-side
scripts/start_sot_online_walking.py
View file @ d26f4fb8
......@@ -14,32 +14,30 @@ from gazebo_msgs.srv import *
from std_srvs.srv import Empty
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
class TestSoTTalos(unittest.TestCase):
class TestSoTTalos(unittest.TestCase):
def validation_through_gazebo(self):
gzGetModelPropReq = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
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")
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))
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.05:
self.assertTrue(True,msg="Converged to the desired position")
if ldistance < 0.05:
self.assertTrue(True, msg="Converged to the desired position")
else:
self.assertFalse(True,
msg="Did not converged to the desired position")
self.assertFalse(True, msg="Did not converged to the desired position")
def runTest(self):
# Start roscore
......@@ -56,10 +54,9 @@ class TestSoTTalos(unittest.TestCase):
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'
]
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)]
......@@ -70,8 +67,7 @@ class TestSoTTalos(unittest.TestCase):
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 = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
gazebo_pause_physics()
time.sleep(5)
......@@ -97,7 +93,7 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path=arospack.get_path('roscontrol_sot_talos')
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'])
......@@ -105,13 +101,13 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("roscontrol_sot_talos started")
time.sleep(5)
pkg_name='talos_integration_tests'
executable='test_online_walking.py'
node_name='test_online_walking_py'
pkg_name = 'talos_integration_tests'
executable = 'test_online_walking.py'
node_name = 'test_online_walking_py'
test_sot_online_walking_node = roslaunch.core.\
Node(pkg_name, executable,name=node_name)
launch_test_sot_online_walking=roslaunch.scriptapi.ROSLaunch()
launch_test_sot_online_walking = roslaunch.scriptapi.ROSLaunch()
launch_test_sot_online_walking.start()
test_sot_online_walking_process = launch_test_sot_online_walking.\
......@@ -124,13 +120,12 @@ class TestSoTTalos(unittest.TestCase):
self.validation_through_gazebo()
# If it is finished then find exit status.
if test_sot_online_walking_process.exit_code != 0:
exit_status = "test_online_walking failed"
self.assertFalse(True,exit_status)
self.assertFalse(True, exit_status)
else:
exit_status=None
exit_status = None
print("Stopping SoT")
launch_roscontrol_sot_talos.shutdown()
......@@ -145,6 +140,7 @@ class TestSoTTalos(unittest.TestCase):
r.sleep()
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG_NAME,'test_online_walking',TestSoTTalos)
rosunit.unitrun(PKG_NAME, 'test_online_walking', TestSoTTalos)
scripts/start_sot_talos_balance.py
View file @ d26f4fb8
......@@ -14,29 +14,27 @@ from gazebo_msgs.srv import *
from std_srvs.srv import Empty
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
class TestSoTTalos(unittest.TestCase):
class TestSoTTalos(unittest.TestCase):
def validation_through_gazebo(self):
gzGetModelPropReq = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
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=gzGetModelPropResp.pose.position.x-2.8331
dy=gzGetModelPropResp.pose.position.y-0.0405
dz=gzGetModelPropResp.pose.position.z-1.0019
ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
f.write("dist:"+str(ldistance))
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 = gzGetModelPropResp.pose.position.x - 2.8331
dy = gzGetModelPropResp.pose.position.y - 0.0405
dz = gzGetModelPropResp.pose.position.z - 1.0019
ldistance = math.sqrt(dx * dx + dy * dy + dz * dz)
f.write("dist:" + str(ldistance))
f.close()
if ldistance<0.009:
self.assertTrue(True,msg="Converged to the desired position")
if ldistance < 0.009:
self.assertTrue(True, msg="Converged to the desired position")
else:
self.assertFalse(True,
msg="Did not converged to the desired position")
self.assertFalse(True, msg="Did not converged to the desired position")
def runTest(self):
# Start roscore
......@@ -53,10 +51,9 @@ class TestSoTTalos(unittest.TestCase):
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'
]
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)]
......@@ -65,8 +62,7 @@ class TestSoTTalos(unittest.TestCase):
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 = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
gazebo_pause_physics()
time.sleep(5)
......@@ -92,7 +88,7 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path=arospack.get_path('roscontrol_sot_talos')
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'])
......@@ -100,13 +96,13 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("roscontrol_sot_talos started")
time.sleep(5)
pkg_name='talos_integration_tests'
executable='test_sot_talos_balance.py'
node_name='test_sot_talos_balance_py'
pkg_name = 'talos_integration_tests'
executable = 'test_sot_talos_balance.py'
node_name = 'test_sot_talos_balance_py'
test_sot_talos_balance_node = roslaunch.core.\
Node(pkg_name, executable,name=node_name)
launch_test_sot_talos_balance=roslaunch.scriptapi.ROSLaunch()
launch_test_sot_talos_balance = roslaunch.scriptapi.ROSLaunch()
launch_test_sot_talos_balance.start()
test_sot_talos_balance_process = launch_test_sot_talos_balance.\
......@@ -119,13 +115,12 @@ class TestSoTTalos(unittest.TestCase):
self.validation_through_gazebo()
# If it is finished then find exit status.
if test_sot_talos_balance_process.exit_code != 0:
exit_status = "test_sot_talos_balance failed"
self.assertFalse(True,exit_status)
self.assertFalse(True, exit_status)
else:
exit_status=None
exit_status = None
print("Stopping SoT")
launch_roscontrol_sot_talos.shutdown()
......@@ -140,6 +135,7 @@ class TestSoTTalos(unittest.TestCase):
r.sleep()
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG_NAME,'test_sot_talos_balance',TestSoTTalos)
rosunit.unitrun(PKG_NAME, 'test_sot_talos_balance', TestSoTTalos)
scripts/start_talos_gazebo.py
View file @ d26f4fb8
......@@ -24,10 +24,7 @@ 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'
]
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)]
......@@ -42,7 +39,7 @@ 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'])
[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()
......@@ -54,18 +51,15 @@ 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 = 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'])
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()
scripts/start_talos_gazebo_kine.py
View file @ d26f4fb8
......@@ -14,23 +14,22 @@ from gazebo_msgs.srv import *
from std_srvs.srv import Empty
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
class TestSoTTalos(unittest.TestCase):
def validation_through_gazebo(self):
gzGetLinkPropReq = rospy.ServiceProxy('/gazebo/get_link_state',GetLinkState)
gzGetLinkPropReq = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
gzGetLinkPropResp = gzGetLinkPropReq(link_name='gripper_right_fingertip_1_link')
f=open("/tmp/output.dat","w+")
f.write("x:"+str(gzGetLinkPropResp.link_state.pose.position.x)+"\n")
f.write("y:"+str(gzGetLinkPropResp.link_state.pose.position.y)+"\n")
f.write("z:"+str(gzGetLinkPropResp.link_state.pose.position.z)+"\n")
dx=gzGetLinkPropResp.link_state.pose.position.x-0.5723
dy=gzGetLinkPropResp.link_state.pose.position.y+0.2885
dz=gzGetLinkPropResp.link_state.pose.position.z-0.7745
ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
f.write("dist:"+str(ldistance))
f = open("/tmp/output.dat", "w+")
f.write("x:" + str(gzGetLinkPropResp.link_state.pose.position.x) + "\n")
f.write("y:" + str(gzGetLinkPropResp.link_state.pose.position.y) + "\n")
f.write("z:" + str(gzGetLinkPropResp.link_state.pose.position.z) + "\n")
dx = gzGetLinkPropResp.link_state.pose.position.x - 0.5723
dy = gzGetLinkPropResp.link_state.pose.position.y + 0.2885
dz = gzGetLinkPropResp.link_state.pose.position.z - 0.7745
ldistance = math.sqrt(dx * dx + dy * dy + dz * dz)
f.write("dist:" + str(ldistance))
f.close()
if ldistance < 0.009:
......@@ -38,7 +37,6 @@ class TestSoTTalos(unittest.TestCase):
else:
self.assertTrue(False)
def runTest(self):
# Start roscore
import subprocess
......@@ -54,9 +52,10 @@ class TestSoTTalos(unittest.TestCase):
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',
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)]
......@@ -91,7 +90,7 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path=arospack.get_path('roscontrol_sot_talos')
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'])
......@@ -99,12 +98,12 @@ class TestSoTTalos(unittest.TestCase):
rospy.loginfo("roscontrol_sot_talos started")
time.sleep(5)
pkg_name='talos_integration_tests'
executable='test_kine.py'
node_name='test_kine_py'
test_kine_node = roslaunch.core.Node(pkg_name, executable,name=node_name)
pkg_name = 'talos_integration_tests'
executable = 'test_kine.py'
node_name = 'test_kine_py'
test_kine_node = roslaunch.core.Node(pkg_name, executable, name=node_name)
launch_test_kine=roslaunch.scriptapi.ROSLaunch()
launch_test_kine = roslaunch.scriptapi.ROSLaunch()
launch_test_kine.start()
test_kine_process = launch_test_kine.launch(test_kine_node)
......@@ -120,7 +119,7 @@ class TestSoTTalos(unittest.TestCase):
if test_kine_process.exit_code != 0:
exit_status = "test_sot_talos_balance failed"
else:
exit_status=None
exit_status = None
print("Stopping SoT")
launch_roscontrol_sot_talos.shutdown()
......@@ -138,4 +137,4 @@ class TestSoTTalos(unittest.TestCase):
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG_NAME,'test_sot_talos_kine',TestSoTTalos)
rosunit.unitrun(PKG_NAME, 'test_sot_talos_kine', TestSoTTalos)
scripts/test_appli_seq_play.py
View file @ d26f4fb8
......@@ -5,8 +5,9 @@ import rospy
from std_srvs.srv import *
from dynamic_graph_bridge_msgs.srv import *
def launchScript(code,title,description = ""):
raw_input(title+': '+description)
def launchScript(code, title, description=""):
raw_input(title + ': ' + description)
rospy.loginfo(title)
rospy.loginfo(code)
for line in code:
......@@ -15,7 +16,7 @@ def launchScript(code,title,description = ""):
answer = runCommandClient(str(line))
rospy.logdebug(answer)
print answer
rospy.loginfo("...done with "+title)
rospy.loginfo("...done with " + title)
# Waiting for services
......@@ -31,12 +32,11 @@ try:
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
initCode = open( "appli_seq_play.py", "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
initCode = open("appli_seq_play.py", "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
launchScript(initCode,'initialize SoT')
launchScript(initCode, 'initialize SoT')
raw_input("Wait before starting the dynamic graph")
runCommandStartDynamicGraph()
......
scripts/test_kine.py
View file @ d26f4fb8
......@@ -4,7 +4,6 @@ import rospy
import time
from os.path import abspath, dirname, join
from std_srvs.srv import *
from dynamic_graph_bridge_msgs.srv import *
......@@ -12,6 +11,7 @@ runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
from sot_talos_balance.utils.run_test_utils import runCommandClient
def handleRunCommandClient(code):
out = runCommandClient(code)
......@@ -19,7 +19,9 @@ def handleRunCommandClient(code):
print("standarderror: " + out.standarderror)
sys.exit(-1)
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
def runTest():
# Waiting for services
......@@ -36,20 +38,20 @@ def runTest():
rospy.wait_for_service('/gazebo/get_link_state')
rospy.loginfo("...ok")
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
Empty)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
rospy.loginfo("Stack of Tasks launched")
handleRunCommandClient('from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d')
handleRunCommandClient('robot.taskRH = MetaTaskKine6d(\'rh\',robot.dynamic,\'rh\',robot.OperationalPointsMap[\'right-wrist\'])')
handleRunCommandClient(
'robot.taskRH = MetaTaskKine6d(\'rh\',robot.dynamic,\'rh\',robot.OperationalPointsMap[\'right-wrist\'])')
handleRunCommandClient('from dynamic_graph.sot.core.sot import SOT')
handleRunCommandClient('robot.sot = SOT(\'sot\')')
handleRunCommandClient('from talos_integration_tests.appli import init_appli')
handleRunCommandClient('init_appli(robot)')
handleRunCommandClient('from dynamic_graph.sot.core.meta_tasks_kine import gotoNd')
runCommandStartDynamicGraph()
handleRunCommandClient("target = (0.5,-0.2,1.0)")
......@@ -61,5 +63,6 @@ def runTest():
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
if __name__ == '__main__':
runTest()
scripts/test_online_walking.py
View file @ d26f4fb8
......@@ -11,6 +11,7 @@ from dynamic_graph_bridge_msgs.srv import *
from gazebo_msgs.srv import *
def handleRunCommandClient(code):
out = runCommandClient(code)
......@@ -19,8 +20,7 @@ def handleRunCommandClient(code):
sys.exit(-1)
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
'''Test online walking pattern generator'''
from sys import argv
......@@ -29,6 +29,7 @@ from sot_talos_balance.utils.run_test_utils import \
from time import sleep
def wait_for_dynamic_graph():
try:
rospy.loginfo("Waiting for run_command")
......@@ -53,8 +54,7 @@ print(lpath)
appli_file_name =lpath + '/../../lib/'+PKG_NAME+\
'/appli_online_walking.py'
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',Empty)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
rospy.loginfo("Stack of Tasks launched")
wait_for_dynamic_graph()
......@@ -68,21 +68,16 @@ runCommandStartDynamicGraph()
rospy.loginfo("Stack of Tasks launched")
#run_test(appli_file_name, verbosity=1,interactive=False)
time.sleep(5)
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
handleRunCommandClient('from dynamic_graph import plug')
handleRunCommandClient(
'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
handleRunCommandClient(
'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
handleRunCommandClient(
'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])'
)
handleRunCommandClient('Kp_adm = [15.0, 15.0, 0.0]')# this value is employed later
handleRunCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
handleRunCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
handleRunCommandClient('robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])')
handleRunCommandClient('Kp_adm = [15.0, 15.0, 0.0]') # this value is employed later
handleRunCommandClient('robot.com_admittance_control.Kp.value = Kp_adm')
handleRunCommandClient('robot.dcm_control.resetDcmIntegralError()')
handleRunCommandClient('Ki_dcm = [1.0, 1.0, 1.0]') # this value is employed later
......
scripts/test_sot_talos_balance.py
View file @ d26f4fb8
......@@ -17,8 +17,8 @@ def handleRunCommandClient(code):
print("standarderror: " + out.standarderror)
sys.exit(-1)
PKG_NAME='talos_integration_tests'
PKG_NAME = 'talos_integration_tests'
'''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
......@@ -39,10 +39,9 @@ rospy.loginfo("...ok")
handleRunCommandClient('test_folder = "' + test_folder + '"')
handleRunCommandClient('from talos_integration_tests.appli_dcmZmpControl_file import init_sot_talos_balance')
handleRunCommandClient('init_sot_talos_balance(robot,\''+test_folder+'\')')
handleRunCommandClient('init_sot_talos_balance(robot,\'' + test_folder + '\')')
time.sleep(5)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
Empty)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
runCommandStartDynamicGraph()
time.sleep(5)
......@@ -52,7 +51,7 @@ handleRunCommandClient('from dynamic_graph import plug')
handleRunCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
handleRunCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
handleRunCommandClient('robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])')
handleRunCommandClient('Kp_adm = [15.0, 15.0, 0.0]')# this value is employed later
handleRunCommandClient('Kp_adm = [15.0, 15.0, 0.0]') # this value is employed later
handleRunCommandClient('robot.com_admittance_control.Kp.value = Kp_adm')
handleRunCommandClient('robot.dcm_control.resetDcmIntegralError()')
handleRunCommandClient('Ki_dcm = [1.0, 1.0, 1.0]') # this value is employed later
......
src/talos_integration_tests/appli.py
View file @ d26f4fb8
......@@ -2,9 +2,11 @@ from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineC
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
def init_appli(robot):
taskRH = MetaTaskKine6d('rh',robot.dynamic,'rh',robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0)
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# --- STATIC COM (if not walking)
......@@ -13,15 +15,14 @@ def init_appli(robot):
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')