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
Guilhem Saurel
hpp-rbprm-corba
Commits
0bfdd9c4
Commit
0bfdd9c4
authored
Jul 25, 2019
by
Pierre Fernbach
Browse files
[script] fix writing of status file for memmo scripts
parent
5d2a9e8b
Changes
9
Hide whitespace changes
Inline
Side-by-side
script/scenarios/memmo/talos_circle_oriented_path.py
View file @
0bfdd9c4
...
...
@@ -4,7 +4,7 @@ from hpp.corbaserver import ProblemSolver
import
numpy
as
np
from
pinocchio
import
Quaternion
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
vMax
=
0.5
# linear velocity bound for the root
...
...
script/scenarios/memmo/talos_circle_path.py
View file @
0bfdd9c4
...
...
@@ -3,7 +3,7 @@ from hpp.gepetto import Viewer
from
hpp.corbaserver
import
ProblemSolver
import
numpy
as
np
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
vMax
=
0.5
# linear velocity bound for the root
...
...
script/scenarios/memmo/talos_moveEffector_flat.py
View file @
0bfdd9c4
...
...
@@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver
import
os
import
random
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
...
...
script/scenarios/memmo/talos_moveEffector_stairs_m10.py
View file @
0bfdd9c4
...
...
@@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver
import
os
import
random
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
...
...
script/scenarios/memmo/talos_moveEffector_stairs_m15.py
View file @
0bfdd9c4
...
...
@@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver
import
os
import
random
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
...
...
script/scenarios/memmo/talos_moveEffector_stairs_p10.py
View file @
0bfdd9c4
...
...
@@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver
import
os
import
random
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
...
...
script/scenarios/memmo/talos_moveEffector_stairs_p15.py
View file @
0bfdd9c4
...
...
@@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver
import
os
import
random
import
time
statusFilename
=
"infos.log"
statusFilename
=
"
/res/
infos.log"
...
...
script/scenarios/memmo/talos_platform_random.py
View file @
0bfdd9c4
...
...
@@ -5,8 +5,19 @@ import time
print
"Plan guide trajectory ..."
import
scenarios.memmo.talos_platform_random_path
as
tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename
=
tp
.
statusFilename
pId
=
0
f
=
open
(
statusFilename
,
"a"
)
if
tp
.
ps
.
numberPaths
()
>
0
:
print
"Path planning OK."
f
.
write
(
"Planning_success: True"
+
"
\n
"
)
f
.
close
()
else
:
print
"Error during path planning"
f
.
write
(
"Planning_success: False"
+
"
\n
"
)
f
.
close
()
import
sys
sys
.
exit
(
1
)
fullBody
=
Robot
()
...
...
@@ -87,7 +98,7 @@ q_goal[configSize+3:configSize+6] = [0,0,0]
fullBody
.
setStaticStability
(
True
)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
#
v.addLandmark('talos/base_link',0.3)
# FOR EASY SCENARIOS ?
q_init
[
2
]
=
q_ref
[
2
]
...
...
@@ -122,7 +133,30 @@ print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if
len
(
configs
)
<
2
:
cg_success
=
False
print
"Error during contact generation."
else
:
cg_success
=
True
print
"Contact generation Done."
if
abs
(
configs
[
-
1
][
0
]
-
tp
.
q_goal
[
0
])
<
0.01
and
abs
(
configs
[
-
1
][
1
]
-
tp
.
q_goal
[
1
])
<
0.01
and
(
len
(
fullBody
.
getContactsVariations
(
len
(
configs
)
-
2
,
len
(
configs
)
-
1
))
==
1
):
print
"Contact generation successful."
cg_reach_goal
=
True
else
:
print
"Contact generation failed to reach the goal."
cg_reach_goal
=
False
f
=
open
(
statusFilename
,
"a"
)
f
.
write
(
"cg_success: "
+
str
(
cg_success
)
+
"
\n
"
)
f
.
write
(
"cg_reach_goal: "
+
str
(
cg_reach_goal
)
+
"
\n
"
)
f
.
close
()
if
(
not
cg_success
):
import
sys
sys
.
exit
(
1
)
# put back original bounds for wholebody methods
fullBody
.
resetJointsBounds
()
displayContactSequence
(
v
,
configs
)
#
displayContactSequence(v,configs)
script/scenarios/memmo/talos_platform_random_path.py
View file @
0bfdd9c4
...
...
@@ -5,7 +5,8 @@ from pinocchio import Quaternion
import
numpy
as
np
import
time
import
math
statusFilename
=
"infos.log"
statusFilename
=
"/res/infos.log"
vInit
=
0.05
# initial / final velocity to fix the direction
vGoal
=
0.01
...
...
@@ -119,12 +120,11 @@ ps.setInitialConfig (q_init)
ps
.
addGoalConfig
(
q_goal
)
# write problem in files :
"""
f
=
open
(
statusFilename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
q_goal
)
+
"
\n
"
)
f
.
close
()
"""
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
...
...
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