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
ecd630be
Commit
ecd630be
authored
May 21, 2020
by
Olivier Stasse
Committed by
olivier stasse
May 25, 2020
Browse files
[scripts] Implements new organization in python modules
Additionnally fix wrong use of assert. The tests are more robust.
parent
4cbb7f44
Changes
4
Hide whitespace changes
Inline
Side-by-side
scripts/start_sot_online_walking.py
View file @
ecd630be
...
...
@@ -26,13 +26,13 @@ class TestSoTTalos(unittest.TestCase):
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.0
405
dz
=
gzGetModelPropResp
.
pose
.
position
.
z
-
1.001
9
dx
=
gzGetModelPropResp
.
pose
.
position
.
x
-
2.
1045
dy
=
gzGetModelPropResp
.
pose
.
position
.
y
-
0.0
038
dz
=
gzGetModelPropResp
.
pose
.
position
.
z
-
1.001
52
ldistance
=
math
.
sqrt
(
dx
*
dx
+
dy
*
dy
+
dz
*
dz
)
f
.
write
(
"dist:"
+
str
(
ldistance
))
f
.
close
()
if
ldistance
<
0.0
09
:
if
ldistance
<
0.0
5
:
self
.
assertTrue
(
True
,
msg
=
"Converged to the desired position"
)
else
:
self
.
assertFalse
(
True
,
...
...
scripts/test_kine.py
View file @
ecd630be
...
...
@@ -41,14 +41,20 @@ def runTest():
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
(
'from dynamic_graph.sot.core.sot import SOT'
)
handleRunCommandClient
(
'robot.sot = SOT(
\'
sot
\'
)'
)
handleRunCommandClient
(
'from talos_integration_tests.appli import init_appli'
)
handleRunCommandClient
(
'init_sot_talos_balance(robot)'
)
launchScript
(
initCode
,
'initialize SoT'
)
handleRunCommandStartDynamicGraph
()
handleRunCommandClient
(
'init_appli(robot)'
)
handleRunCommandClient
(
'from dynamic_graph.sot.core.meta_tasks_kine import gotoNd'
)
runCommandStartDynamicGraph
()
handleRunCommandClient
(
"target = (0.5,-0.2,1.0)"
)
handleRunCommandClient
(
"gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))"
)
handleRunCommandClient
(
"sot.push(taskRH.task.name)"
)
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
)
...
...
scripts/test_online_walking.py
View file @
ecd630be
...
...
@@ -98,5 +98,6 @@ 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
(
5
)
time
.
sleep
(
9
)
handleRunCommandClient
(
'from sot_talos_balance.create_entities_utils import dump_tracer'
)
handleRunCommandClient
(
'dump_tracer(robot.tracer)'
)
scripts/test_sot_talos_balance.py
View file @
ecd630be
...
...
@@ -45,6 +45,7 @@ runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
Empty
)
runCommandStartDynamicGraph
()
time
.
sleep
(
5
)
# Connect ZMP reference and reset controllers
print
(
'Connect ZMP reference'
)
handleRunCommandClient
(
'from dynamic_graph import plug'
)
...
...
@@ -58,7 +59,6 @@ handleRunCommandClient('Ki_dcm = [1.0, 1.0, 1.0]') # this value is employed lat
handleRunCommandClient
(
'robot.dcm_control.Ki.value = Ki_dcm'
)
print
(
'Executing the trajectory'
)
#
time.sleep(1)
time
.
sleep
(
1
)
handleRunCommandClient
(
'robot.triggerTrajGen.sin.value = 1'
)
time
.
sleep
(
2500
)
#handleRunCommandClient('dump_tracer(robot.tracer)')
time
.
sleep
(
25
)
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment