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
Humanoid Path Planner
hpp-rbprm-corba
Commits
d8bc3c9d
Commit
d8bc3c9d
authored
Oct 16, 2019
by
Pierre Fernbach
Committed by
Pierre Fernbach
Jan 17, 2020
Browse files
Maze easy example
parent
80fa6d8f
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/scenarios/memmo/talos_mazeEas_path.py
View file @
d8bc3c9d
...
...
@@ -13,7 +13,7 @@ mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Robot
()
# Define bounds for the root : bounding box of the scenario
root_bounds
=
[
0
,
1
2
,
0.
,
2.
,
0.98
,
0.98
]
root_bounds
=
[
0
,
1
8.5
,
0.
,
2
4
.
,
0.98
,
0.98
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
# As this scenario only consider walking, we fix the DOF of the torso :
rbprmBuilder
.
setJointBounds
(
'torso_1_joint'
,
[
0
,
0
])
...
...
@@ -99,16 +99,24 @@ def generate_random_conf(bounds):
print
"Getting invalid config. try again."
print
message
#set init
q_init
=
generate_random_conf
(
root_bounds
)
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
print
"Press ENTER to continue"
raw_input
()
while
(
True
):
q_init
=
generate_random_conf
(
root_bounds
)
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
print
"Accept?y/n"
user_in
=
raw_input
()
if
user_in
==
'y'
:
break
#set goal
q_goal
=
generate_random_conf
(
root_bounds
)
v
(
q_goal
)
print
"Press ENTER to continue"
raw_input
()
while
(
True
):
q_goal
=
generate_random_conf
(
root_bounds
)
v
(
q_goal
)
print
"Accept?y/n"
user_in
=
raw_input
()
if
user_in
==
'y'
:
break
ps
.
resetGoalConfigs
()
ps
.
addGoalConfig
(
q_goal
)
...
...
@@ -127,6 +135,14 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
#ps.setMaxIterPathPlanning(1000)
t
=
ps
.
solve
()
#tic = time.time()
#v.solveAndDisplay('rm',5,0.01)
#ps.optimizePath(0)
#print "solve in " + str(time.time()-tic)
#raw_input()
print
"Guide planning time : "
,
t
...
...
script/scenarios/memmo/talos_randomMove_flat.py
View file @
d8bc3c9d
...
...
@@ -108,7 +108,10 @@ floor_Z = 0.
s0
=
sampleRandomStateFlatFloor
(
fullBody
,
limbsInContact
,
floor_Z
)
q_init
=
s0
.
q
()
<<<<<<<
HEAD
=======
>>>>>>>
73
ceb29917481d69e8c03ef7bba36d22e18d9958
q_goal
=
q_ref
[::]
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
q_goal
[
2
]
=
q_ref
[
2
]
...
...
script/tools/surfaces_from_path.py
View file @
d8bc3c9d
...
...
@@ -86,6 +86,7 @@ def getMergedPhases (seqs):
nseqs
.
append
(
nseq
)
return
nseqs
def
computeRootYawAngleBetwwenConfigs
(
q0
,
q1
):
quat0
=
Quaternion
(
q0
[
6
],
q0
[
3
],
q0
[
4
],
q0
[
5
])
quat1
=
Quaternion
(
q1
[
6
],
q1
[
3
],
q1
[
4
],
q1
[
5
])
...
...
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