test_sot_talos_balance.py 2.27 KB
Newer Older
1
2
3
4
5
6
#! /usr/bin/env python
import sys
import rospy
import time
import unittest
import math
Guilhem Saurel's avatar
Guilhem Saurel committed
7
from os.path import abspath, dirname, join
8
9
10
11

from std_srvs.srv import *
from dynamic_graph_bridge_msgs.srv import *

12
13
14
15
16
17
18

def handleRunCommandClient(code):
    out = runCommandClient(code)

    if out.standarderror:
        print("standarderror: " + out.standarderror)
        sys.exit(-1)
19
20
21
22

PKG_NAME='talos_integration_tests'

'''Test CoM admittance control as described in paper, with pre-loaded movements'''
23
from sot_talos_balance.utils.run_test_utils import  \
24
25
    run_ft_calibration, run_test, runCommandClient

26
# get the file path for rospy_tutorials
Guilhem Saurel's avatar
Guilhem Saurel committed
27
appli_file_name = join(dirname(abspath(__file__)), 'appli_dcmZmpControl_file.py')
28
29
30
31
32
33
34
35
36
37
38

time.sleep(2)
rospy.loginfo("Stack of Tasks launched")

test_folder = 'TestKajita2003StraightWalking64/20cm'
print('Using folder ' + test_folder)

rospy.loginfo("Waiting for run_command")
rospy.wait_for_service('/run_command')
rospy.loginfo("...ok")

39
handleRunCommandClient('test_folder = "' + test_folder + '"')
40

41
42
handleRunCommandClient('from talos_integration_tests.appli_dcmZmpControl_file import init_sot_talos_balance')
handleRunCommandClient('init_sot_talos_balance(robot,\''+test_folder+'\')')
43
time.sleep(5)
44
45
46
47
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
                                                 Empty)

runCommandStartDynamicGraph()
48
time.sleep(5)
49
50
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
51
52
53
54
55
56
57
58
59
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('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
handleRunCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
60
61

print('Executing the trajectory')
62
time.sleep(1)
63
handleRunCommandClient('robot.triggerTrajGen.sin.value = 1')
64
time.sleep(25)