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
156d8225
Commit
156d8225
authored
Sep 03, 2020
by
Pierre Fernbach
Browse files
[Scenario] add scenario class for Solo and add demos scripts
parent
993c64c2
Changes
9
Hide whitespace changes
Inline
Side-by-side
src/CMakeLists.txt
View file @
156d8225
...
...
@@ -119,6 +119,8 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS
__init__.py
talos_contact_generator.py
talos_path_planner.py
solo_contact_generator.py
solo_path_planner.py
)
SET
(
${
PROJECT_NAME
}
_PYTHON_SCENARIOS_DEMOS
...
...
@@ -143,6 +145,12 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_plateformes.py
talos_stairs10_path.py
talos_stairs10.py
talos_stairs15_path.py
talos_stairs15.py
solo_flatGround_path.py
solo_flatGround.py
solo_slalom_path.py
solo_pallet_path.py
)
SET
(
${
PROJECT_NAME
}
_PYTHON_SCENARIOS_MEMMO
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/solo_flatGround.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.demos.solo_flatGround_path
import
PathPlanner
from
hpp.corbaserver.rbprm.scenarios.solo_contact_generator
import
SoloContactGenerator
class
ContactGenerator
(
SoloContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
testReachability
=
False
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_flatGround_path.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
0
,
0
]
self
.
q_goal
[:
2
]
=
[
0.5
,
0
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
(
True
,
False
)
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_pallet_path.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
0.85
,
0.
]
# self.q_goal[:2] = [1, 0.]
self
.
q_goal
[:
2
]
=
[
0.
,
0.
]
self
.
init_viewer
(
"ori/modular_palet_low"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.06
,
0
,
0
],
min_area
=
[[
'Support'
,
0.05
]])
self
.
init_planner
(
True
,
False
)
self
.
ps
.
directPath
(
self
.
q_init
,
self
.
q_goal
,
False
)
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_slalom_path.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
0.7
,
0.95
]
#self.q_goal[:2] = [1.05, 0.95]
self
.
q_goal
[:
2
]
=
[
-
0.2
,
0.95
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self
.
init_viewer
(
"multicontact/slalom_debris"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.05
,
0
,
0
])
self
.
init_planner
(
True
,
False
)
self
.
solve
()
"""
ps = self.ps
q_init = self.rbprmBuilder.getCurrentConfig();
q_init[0:3] = [-1.8, 0., self.rbprmBuilder.ref_height];
self.v(q_init)
q_init[-6:-3] = [0.1, 0, 0]
q_goal = q_init[::]
q_goal[0:3] = [-0.9, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
q_init_0 = q_init[::]
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_begin = ps.numberPaths() - 1
### END BEGIN up to the rubbles #####
ps.resetGoalConfigs()
### BEGIN rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.4)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
# q_init[-6:-3] = [0.,0,0]
q_goal[0:3] = [1.05, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
for i in range(10):
ps.optimizePath(ps.numberPaths() -1)
pId_rubbles = ps.numberPaths() - 1
### END rubbles #####
ps.resetGoalConfigs()
### BEGIN after rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.15)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
q_goal[0:3] = [2.2, 0, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.05, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_end = ps.numberPaths() - 1
### END after rubbles #####
self.pathId = pId_begin
ps.concatenatePath(self.pathId, pId_rubbles)
ps.concatenatePath(self.pathId, pId_end)
"""
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs15.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.demos.talos_stairs15_path
import
PathPlanner
from
hpp.corbaserver.rbprm.scenarios.talos_contact_generator
import
TalosContactGenerator
class
ContactGenerator
(
TalosContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
test_reachability
=
False
def
load_fullbody
(
self
):
super
().
load_fullbody
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig_elbowsUp
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
# path planning use different limbs, reset it to right/left feet
self
.
used_limbs
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
rArmId
]
self
.
init_contacts
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
]
self
.
end_contacts
=
self
.
used_limbs
def
load_limbs
(
self
):
super
().
load_limbs
(
"static"
,
nb_samples
=
100000
)
def
compute_configs_from_guide
(
self
):
super
().
compute_configs_from_guide
()
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
+
0.75
# set height to the top of the stairs
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs15_path.py
0 → 100644
View file @
156d8225
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfNameRom
=
[
Robot
.
lLegId
,
Robot
.
rLegId
,
Robot
.
rArmId
]
self
.
used_limbs
=
Robot
.
urdfNameRom
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
self
.
ps
.
setParameter
(
"Kinodynamic/synchronizeVerticalAxis"
,
True
)
self
.
ps
.
setParameter
(
"Kinodynamic/verticalAccelerationBound"
,
3.
)
def
compute_extra_config_bounds
(
self
):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
-
10
,
10
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
-
10
,
10
]
def
run
(
self
):
self
.
root_translation_bounds
=
[
-
2.
,
-
1.7
,
0
,
2.
,
0.95
,
2.
]
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
1.9
,
1.9
]
self
.
q_init
[
3
:
7
]
=
[
0
,
0
,
-
0.7071
,
0.7071
]
self
.
q_init
[
2
]
=
0.98
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
1
]
=
0.9
self
.
q_goal
[
2
]
+=
3
*
0.15
# 4 steps of 15cm each
self
.
init_viewer
(
"multicontact/bauzil_stairs"
,
reduce_sizes
=
[
0.08
,
0.
,
0.
],
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/solo_contact_generator.py
0 → 100644
View file @
156d8225
from
.abstract_contact_generator
import
AbstractContactGenerator
class
SoloContactGenerator
(
AbstractContactGenerator
):
def
__init__
(
self
,
path_planner
):
super
().
__init__
(
path_planner
)
self
.
robustness
=
5
self
.
used_limbs
=
[
'FRleg'
,
'HLleg'
,
'FLleg'
,
'HRleg'
]
self
.
init_contacts
=
self
.
used_limbs
[::]
self
.
end_contacts
=
self
.
used_limbs
[::]
self
.
robot_node_name
=
"solo"
def
set_start_end_states
(
self
):
# for 3D contacts, we must specify the normals manually
normals_init
=
[]
normals_end
=
[]
for
limb_id
in
self
.
init_contacts
:
normals_init
+=
[
self
.
fullbody
.
dict_normal
[
self
.
fullbody
.
dict_limb_joint
[
limb_id
]]]
for
limb_id
in
self
.
init_contacts
:
normals_end
+=
[
self
.
fullbody
.
dict_normal
[
self
.
fullbody
.
dict_limb_joint
[
limb_id
]]]
self
.
fullbody
.
setStartState
(
self
.
q_init
,
self
.
init_contacts
,
normals_init
)
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
,
normals_end
)
def
load_fullbody
(
self
):
from
solo_rbprm.solo
import
Robot
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
fullbody
.
setConstrainedJointsBounds
()
def
init_viewer
(
self
):
super
().
init_viewer
()
self
.
v
.
addLandmark
(
'solo/base_link_0'
,
0.3
)
def
load_limbs
(
self
,
heuristic
=
"fixedStep04"
,
analysis
=
None
,
nb_samples
=
None
,
octree_size
=
None
):
super
().
load_limbs
(
heuristic
,
analysis
,
nb_samples
,
octree_size
,
disableEffectorCollision
=
True
)
src/hpp/corbaserver/rbprm/scenarios/solo_path_planner.py
0 → 100644
View file @
156d8225
from
.abstract_path_planner
import
AbstractPathPlanner
class
SoloPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.241
,
0.241
]
# set default used limbs to be both feet
self
.
used_limbs
=
[
'solo_RFleg_rom'
,
'solo_LHleg_rom'
,
'solo_LFleg_rom'
,
'solo_RHleg_rom'
]
self
.
size_foot_x
=
0.01
# size of the feet along the x axis
self
.
size_foot_y
=
0.01
# size of the feet along the y axis
self
.
v_max
=
0.3
self
.
a_max
=
1.
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
self
.
robot_node_name
=
"solo_trunk"
def
load_rbprm
(
self
):
from
solo_rbprm.solo_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
def
set_configurations
(
self
):
super
().
set_configurations
()
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