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
44207e1a
Commit
44207e1a
authored
Mar 05, 2020
by
Pierre Fernbach
Browse files
[Scripts] refactor all HYQ scripts in demos/
parent
afd0446c
Changes
9
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/darpa_hyq.py
deleted
100644 → 0
View file @
afd0446c
#Importing helper class for RBPRM
from
hpp.corbaserver.rbprm.hyq_contact6D
import
Robot
from
hpp.corbaserver.problem_solver
import
ProblemSolver
from
hpp.gepetto
import
Viewer
#calling script darpa_hyq_path to compute root path
from
.
import
darpa_hyq_path
as
tp
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_HPP_DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
from
hpp.corbaserver
import
Client
# This time we load the full body model of HyQ
fullBody
=
Robot
()
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
#~ fullBody.client.robot.setDimensionExtraConfigSpace(6)
#~ fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples
=
10000
ps
=
tp
.
ProblemSolver
(
fullBody
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
)
rootName
=
'base_joint_xyz'
cType
=
"_6_DOF"
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.1
,
cType
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.05
,
cType
)
fullBody
.
addLimb
(
fullBody
.
rArmId
,
fullBody
.
rarm
,
fullBody
.
rhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.05
,
cType
)
fullBody
.
addLimb
(
fullBody
.
lArmId
,
fullBody
.
larm
,
fullBody
.
lhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"random"
,
0.05
,
cType
)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_init
=
fullBody
.
referenceConfig
[::]
;
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
];
q_goal
=
fullBody
.
referenceConfig
[::];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
# specifying the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
])
v
(
q_init
)
configs
=
[]
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
,
v
)
import
time
#DEMO METHODS
def
initConfig
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_init
)
def
endConfig
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_goal
)
def
rootPath
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"ON"
)
tp
.
pp
(
0
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
(
stepsize
=
0.06
):
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
global
configs
start
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
stepsize
,
5
,
0.
,
filterStates
=
True
,
testReachability
=
False
)
end
=
time
.
time
()
print
((
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
))
def
contactPlan
(
step
=
0.5
):
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)):
v
(
configs
[
i
]);
time
.
sleep
(
step
)
print
((
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
))
genPlan
(
0.01
);
contactPlan
(
0.01
)
script/scenarios/demos/darpa_hyq3d.py
deleted
100644 → 0
View file @
afd0446c
#Importing helper class for RBPRM
from
hpp.corbaserver.rbprm.hyq
import
Robot
from
hpp.corbaserver.problem_solver
import
ProblemSolver
from
hpp.gepetto
import
Viewer
#calling script darpa_hyq_path to compute root path
from
.
import
darpa_hyq_path
as
tp
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_HPP_DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
from
hpp.corbaserver
import
Client
# This time we load the full body model of HyQ
fullBody
=
Robot
()
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
6
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
# Setting a number of sample configurations used
nbSamples
=
10000
ps
=
tp
.
ProblemSolver
(
fullBody
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
)
rootName
=
'base_joint_xyz'
cType
=
"_3_DOF"
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.1
,
cType
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.05
,
cType
)
fullBody
.
addLimb
(
fullBody
.
rArmId
,
fullBody
.
rarm
,
fullBody
.
rhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"forward"
,
0.05
,
cType
)
fullBody
.
addLimb
(
fullBody
.
lArmId
,
fullBody
.
larm
,
fullBody
.
lhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"random"
,
0.05
,
cType
)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_init
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
;
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
];
#q_init[2]=fullBody.referenceConfig[2]
q_goal
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
;
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
#q_goal[2]=fullBody.referenceConfig[2]
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
# specifying the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
],
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
],
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)])
v
(
q_init
)
configs
=
[]
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
,
v
)
import
time
#DEMO METHODS
def
initConfig
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_init
)
def
endConfig
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_goal
)
def
rootPath
():
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"ON"
)
tp
.
pp
(
0
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
(
stepsize
=
0.06
):
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
global
configs
start
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
stepsize
,
5
,
0.
,
filterStates
=
True
,
testReachability
=
False
)
end
=
time
.
time
()
print
(
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
)
def
contactPlan
(
step
=
0.5
):
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)):
v
(
configs
[
i
]);
time
.
sleep
(
step
)
print
(
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
)
genPlan
(
0.01
);
contactPlan
(
0.01
)
script/scenarios/demos/darpa_hyq_path.py
deleted
100644 → 0
View file @
afd0446c
# Importing helper class for setting up a reachability planning problem
from
hpp.corbaserver.rbprm.hyq_abstract
import
Robot
# Importing Gepetto viewer helper class
from
hpp.gepetto
import
Viewer
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Robot
()
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder
.
setFilter
([
'hyq_rhleg_rom'
,
'hyq_lfleg_rom'
,
'hyq_rfleg_rom'
,
'hyq_lhleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_rhleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_rfleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_lhleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_lfleg_rom'
,
[
'Support'
,])
# We also bound the rotations of the torso.
rbprmBuilder
.
boundSO3
([
-
0.4
,
0.4
,
-
0.3
,
0.3
,
-
0.3
,
0.3
])
# Creating an instance of HPP problem solver and the viewer
from
hpp.corbaserver.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/darpa"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
,
0
,
0
])
v
=
vf
.
createViewer
()
#afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.64
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
v
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.64
];
v
(
q_goal
)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
ps
.
addPathOptimizer
(
"RandomShortcut"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps
.
client
.
problem
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Solve the problem
t
=
ps
.
solve
()
#~ t = 0.
if
isinstance
(
t
,
list
):
t
=
t
[
0
]
*
3600000
+
t
[
1
]
*
60000
+
t
[
2
]
*
1000
+
t
[
3
]
print
((
"computation time for root path "
,
t
))
# Playing the computed path
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
q_far
=
q_init
[::]
q_far
[
0
:
3
]
=
[
-
2
,
-
3
,
0.6
];
v
(
q_far
)
for
i
in
range
(
1
,
10
):
rbprmBuilder
.
client
.
problem
.
optimizePath
(
i
)
#~ pp(9)
from
hpp.corbaserver
import
Client
#DEMO code to play root path and final contact plan
cl
=
Client
()
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
rbprmBuilder2
=
Robot
(
"toto"
)
ps2
=
ProblemSolver
(
rbprmBuilder2
)
cl
.
problem
.
selectProblem
(
"default"
)
cl
.
problem
.
movePathToProblem
(
8
,
"rbprm_path"
,
rbprmBuilder
.
getAllJointNames
()[
1
:])
r2
=
Viewer
(
ps2
,
viewerClient
=
v
.
client
)
r2
(
q_far
)
script/scenarios/demos/hyq_darpa.py
0 → 100644
View file @
44207e1a
from
scenarios.demos.hyq_darpa_path
import
PathPlanner
from
scenarios.hyq_contact_generator
import
HyqContactGenerator
class
ContactGenerator
(
HyqContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
def
load_limbs
(
self
):
dict_heuristic
=
{
self
.
fullbody
.
rLegId
:
"fixedStep04"
,
self
.
fullbody
.
lLegId
:
"fixedStep04"
,
self
.
fullbody
.
rArmId
:
"static"
,
self
.
fullbody
.
lArmId
:
"static"
}
super
().
load_limbs
(
dict_heuristic
,
"ReferenceConfiguration"
)
def
compute_configs_from_guide
(
self
):
super
().
compute_configs_from_guide
()
self
.
q_init
[
2
]
=
self
.
q_ref
[
2
]
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
cg
.
play_guide_path
()
cg
.
display_sequence
()
\ No newline at end of file
script/scenarios/demos/hyq_darpa_path.py
0 → 100644
View file @
44207e1a
from
scenarios.hyq_path_planner
import
HyqPathPlanner
class
PathPlanner
(
HyqPathPlanner
):
def
init_problem
(
self
):
super
().
init_problem
()
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
def
run
(
self
):
self
.
root_translation_bounds
=
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
]
self
.
root_rotation_bounds
=
[
-
0.4
,
0.4
,
-
0.3
,
0.3
,
-
0.3
,
0.3
]
self
.
init_problem
()
self
.
q_init
[
0
:
2
]
=
[
-
2
,
0
]
self
.
q_goal
[
0
:
2
]
=
[
3
,
0
]
self
.
init_viewer
(
"multicontact/darpa"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.06
,
0
,
0
])
self
.
init_planner
(
kinodynamic
=
False
)
self
.
solve
()
print
(
"Optimize path ..."
)
for
i
in
range
(
1
,
6
):
self
.
rbprmBuilder
.
client
.
problem
.
optimizePath
(
i
)
print
(
"Optimization pass "
+
str
(
i
)
+
"/5 done."
)
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
script/scenarios/demos/hyq_slalom_debris.py
View file @
44207e1a
from
hpp.corbaserver.rbprm.hyq_contact6D
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
(
"Plan guide trajectory ..."
)
from
.
import
hyq_slalom_debris_path
as
tp
print
(
"Done."
)
import
time
from
scenarios.demos.hyq_slalom_debris_path
import
PathPlanner
from
scenarios.hyq_contact_generator
import
HyqContactGenerator
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
class
ContactGenerator
(
HyqContactGenerator
):
# Set the bounds for the root
root_bounds
=
tp
.
root_bounds
root_bounds
[
4
]
-=
0.05
root_bounds
[
5
]
+=
0.05
fullBody
.
setJointBounds
(
"root_joint"
,
root_bounds
)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
tp
.
vMax
,
tp
.
vMax
,
-
tp
.
vMax
,
tp
.
vMax
,
0
,
0
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMax
,
tp
.
aMax
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
tp
.
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
tp
.
aMax
)
#load the viewer
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
def
load_limbs
(
self
):
super
().
load_limbs
(
"fixedStep06"
,
"ReferenceConfiguration"
)
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
10000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
addLimb
(
fullBody
.
rArmId
,
fullBody
.
rarm
,
fullBody
.
rhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
addLimb
(
fullBody
.
lArmId
,
fullBody
.
larm
,
fullBody
.
lhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"fixedStep06"
,
0.01
)
for
limbId
in
fullBody
.
limbNames
:
fullBody
.
runLimbSampleAnalysis
(
limbId
,
"ReferenceConfiguration"
,
True
)
tGenerate
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
#define initial and final configurations :
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_init
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
dir_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
.
addLandmark
(
'hyq/base_link'
,
0.3
)
q_init
[
2
]
=
fullBody
.
referenceConfig
[
2
]
q_goal
[
2
]
=
fullBody
.
referenceConfig
[
2
]
v
(
q_init
)
#q_init = fullBody.generateContacts(q_init, [0,0,1])
#q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specify the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
rArmId
,
fullBody
.
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
rArmId
,
fullBody
.
lLegId
])
print
(
"Generate contact plan ..."
)
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
testReachability
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Contact plan generated in : "
+
str
(
tInterpolateConfigs
)
+
" s"
)
print
(
"number of configs :"
,
len
(
configs
))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
def
compute_configs_from_guide
(
self
):
super
().
compute_configs_from_guide
()
self
.
q_init
[
2
]
=
self
.
q_ref
[
2
]
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
script/scenarios/demos/hyq_slalom_debris_path.py
View file @
44207e1a
from
hpp.corbaserver.rbprm.hyq_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
time
from
scenarios.hyq_path_planner
import
HyqPathPlanner
class
PathPlanner
(
HyqPathPlanner
):
def
init_problem
(
self
):
super
().
init_problem
()
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
vMax
=
0.2
# linear velocity bound for the root
aMax
=
0.1
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# 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
=
[
-
1.7
,
2.5
,
-
0.2
,
2
,
0.63
,
0.63
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder
.
setFilter
([
'hyq_rhleg_rom'
,
'hyq_lfleg_rom'
,
'hyq_rfleg_rom'
,
'hyq_lhleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_rhleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_rfleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_lhleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hyq_lfleg_rom'
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
4
,
4
,
-
0.1
,
0.1
,
-
0.1
,
0.1
])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
robot
.
getDimensionExtraConfigSpace
()
def
run
(
self
):
self
.
v_max
=
0.2
self
.
a_max
=
0.1
self
.
root_translation_bounds
=
[
-
1.7
,
2.5
,
-
0.2
,
2
,
0.64
,
0.64
]
self
.
init_problem
()
# Creating an instance of HPP problem solver
ps
=
ProblemSolver
(
rbprmBuilder
)
# define parameters used by various methods :
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
self
.
q_init
[
0
:
2
]
=
[
-
1.5
,
0
]
self
.
q_init
[
-
6
]
=
0.05
self
.
q_goal
[
0
:
2
]
=
[
2.2
,
0
]
self
.
q_goal
[
-
6
]
=
0.05
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
self
.
init_viewer
(
"multicontact/slalom_debris"
)
self
.
init_planner
()
self
.
solve
()
self
.
ps
.
optimizePath
(
1
)
self
.
display_path
()