Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
talos_integration_tests
Commits
d26f4fb8
Commit
d26f4fb8
authored
May 28, 2020
by
Guilhem Saurel
Browse files
[Format] yapf
parent
7f93d788
Changes
11
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'
)