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
721735bb
Commit
721735bb
authored
Sep 15, 2020
by
Guilhem Saurel
Browse files
fix for python 2
parent
f408152a
Pipeline
#11721
failed with stage
in 4 minutes and 52 seconds
Changes
5
Pipelines
3
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py
View file @
721735bb
from
abc
import
abstractmethod
from
hpp.gepetto
import
ViewerFactory
,
PathPlayer
from
hpp.corbaserver
import
Client
,
ProblemSolver
,
createContext
,
loadServerPlugin
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
from
hpp.corbaserver
import
ProblemSolver
,
Client
from
hpp.corbaserver
import
createContext
,
loadServerPlugin
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
from
hpp.gepetto
import
PathPlayer
,
ViewerFactory
class
AbstractPathPlanner
:
...
...
@@ -15,7 +16,7 @@ class AbstractPathPlanner:
extra_dof_bounds
=
None
robot_node_name
=
None
# name of the robot in the node list of the viewer
def
__init__
(
self
,
context
=
None
):
def
__init__
(
self
,
context
=
None
):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
...
...
@@ -27,7 +28,7 @@ class AbstractPathPlanner:
0.01
]
# bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self
.
extra_dof
=
6
# number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self
.
mu
=
0.5
#
friction coefficient between the robot and the environment
self
.
mu
=
0.5
#
friction coefficient between the robot and the environment
self
.
used_limbs
=
[]
# names of the limbs that must be in contact during all the motion
self
.
size_foot_x
=
0
# size of the feet along the x axis
self
.
size_foot_y
=
0
# size of the feet along the y axis
...
...
@@ -65,12 +66,10 @@ class AbstractPathPlanner:
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
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
.
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
]
def
set_joints_bounds
(
self
):
"""
...
...
@@ -126,14 +125,17 @@ class AbstractPathPlanner:
vf
=
ViewerFactory
(
self
.
ps
)
if
self
.
context
:
self
.
afftool
=
AffordanceTool
(
context
=
self
.
context
)
self
.
afftool
.
client
.
affordance
.
affordance
.
resetAffordanceConfig
()
# FIXME: this should be called in afftool constructor
self
.
afftool
.
client
.
affordance
.
affordance
.
resetAffordanceConfig
(
)
# FIXME: this should be called in afftool constructor
else
:
self
.
afftool
=
AffordanceTool
()
self
.
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
self
.
afftool
.
loadObstacleModel
(
"package://"
+
env_package
+
"/urdf/"
+
env_name
+
".urdf"
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
self
.
afftool
.
loadObstacleModel
(
"package://"
+
env_package
+
"/urdf/"
+
env_name
+
".urdf"
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
self
.
v
=
vf
.
createViewer
(
ghost
=
True
,
displayArrows
=
True
)
self
.
v
=
vf
.
createViewer
(
ghost
=
True
,
displayArrows
=
True
)
self
.
pp
=
PathPlayer
(
self
.
v
)
for
aff_type
in
visualize_affordances
:
self
.
afftool
.
visualiseAffordances
(
aff_type
,
self
.
v
,
self
.
v
.
color
.
lightBrown
)
...
...
@@ -156,7 +158,7 @@ class AbstractPathPlanner:
else
:
self
.
ps
.
addPathOptimizer
(
"RandomShortcut"
)
def
solve
(
self
,
display_roadmap
=
False
):
def
solve
(
self
,
display_roadmap
=
False
):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
...
...
@@ -174,7 +176,6 @@ class AbstractPathPlanner:
t
=
self
.
ps
.
solve
()
print
(
"Guide planning time : "
,
t
)
def
display_path
(
self
,
path_id
=-
1
,
dt
=
0.1
):
"""
Display the path in the viewer, if no path specified display the last one
...
...
@@ -223,7 +224,7 @@ class AbstractPathPlanner:
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround.py
View file @
721735bb
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.scenarios.demos.talos_flatGround_path import PathPlan
from
hpp.corbaserver.rbprm.scenarios.talos_contact_generator
import
TalosContactGenerator
class
ContactGenerator
(
TalosContactGenerator
):
class
ContactGenerator
(
TalosContactGenerator
,
object
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
super
(
ContactGenerator
,
self
).
__init__
(
PathPlanner
())
if
__name__
==
"__main__"
:
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround_path.py
View file @
721735bb
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
class
PathPlanner
(
TalosPathPlanner
,
object
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
super
(
PathPlanner
,
self
).
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
()
...
...
src/hpp/corbaserver/rbprm/scenarios/talos_contact_generator.py
View file @
721735bb
from
.abstract_contact_generator
import
AbstractContactGenerator
import
time
from
.abstract_contact_generator
import
AbstractContactGenerator
class
TalosContactGenerator
(
AbstractContactGenerator
):
class
TalosContactGenerator
(
AbstractContactGenerator
,
object
):
def
__init__
(
self
,
path_planner
):
super
().
__init__
(
path_planner
)
super
(
TalosContactGenerator
,
self
).
__init__
(
path_planner
)
self
.
robustness
=
2
self
.
robot_node_name
=
"talos"
...
...
@@ -13,8 +14,8 @@ class TalosContactGenerator(AbstractContactGenerator):
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
.
limbs_names
=
[
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
lLegId
]
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
lLegId
]
def
init_viewer
(
self
):
super
().
init_viewer
()
super
(
TalosContactGenerator
,
self
).
init_viewer
()
self
.
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
src/hpp/corbaserver/rbprm/scenarios/talos_path_planner.py
View file @
721735bb
from
.abstract_path_planner
import
AbstractPathPlanner
class
TalosPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
class
TalosPathPlanner
(
AbstractPathPlanner
,
object
):
def
__init__
(
self
,
context
=
None
):
super
(
TalosPathPlanner
,
self
).
__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.95
,
1.05
]
# set default used limbs to be both feet
...
...
@@ -24,11 +24,11 @@ class TalosPathPlanner(AbstractPathPlanner):
self
.
root_translation_bounds
[
-
2
:]
=
[
self
.
rbprmBuilder
.
ref_height
]
*
2
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
super
(
TalosPathPlanner
,
self
).
set_joints_bounds
()
self
.
rbprmBuilder
.
setJointBounds
(
'torso_1_joint'
,
[
0
,
0
])
self
.
rbprmBuilder
.
setJointBounds
(
'torso_2_joint'
,
[
0.006761
,
0.006761
])
def
set_configurations
(
self
):
super
().
set_configurations
()
super
(
TalosPathPlanner
,
self
).
set_configurations
()
self
.
q_init
[
8
]
=
0.006761
self
.
q_goal
[
8
]
=
0.006761
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