Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
H
hpp-rbprm-corba
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
Humanoid Path Planner
hpp-rbprm-corba
Commits
f369d280
Commit
f369d280
authored
8 years ago
by
Akseppal
Browse files
Options
Downloads
Patches
Plain Diff
add test files
parent
c22dce8a
No related branches found
Branches containing commit
No related tags found
2 merge requests
!2
new release of the planner with clean demos / tutorials
,
!3
release of rbprm-corba for ijrr resubmission
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
test1.py
+75
-0
75 additions, 0 deletions
test1.py
test2.py
+64
-0
64 additions, 0 deletions
test2.py
with
139 additions
and
0 deletions
test1.py
0 → 100644
+
75
−
0
View file @
f369d280
# Importing helper class for setting up a reachability planning problem
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
# Importing Gepetto viewer helper class
from
hpp.gepetto
import
Viewer
rootJointType
=
'
freeflyer
'
packageName
=
'
hpp-rbprm-corba
'
meshPackageName
=
'
hpp-rbprm-corba
'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'
hyq_trunk_large
'
# URDF files describing the reachable workspace of each limb of HyQ
urdfNameRom
=
[
'
hyq_lhleg_rom
'
,
'
hyq_lfleg_rom
'
,
'
hyq_rfleg_rom
'
,
'
hyq_rhleg_rom
'
]
urdfSuffix
=
""
srdfSuffix
=
""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"
base_joint_xyz
"
,
[
-
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
'
])
# ... and only if a contact surface includes the gravity
rbprmBuilder
.
setAffordanceFilter
(
'
hyq_lhleg_rom
'
,
[
'
Support
'
,])
rbprmBuilder
.
setAffordanceFilter
(
'
hyq_rfleg_rom
'
,
[
'
Support
'
,])
rbprmBuilder
.
setAffordanceFilter
(
'
hyq_lfleg_rom
'
,
[
'
Support
'
,])
rbprmBuilder
.
setAffordanceFilter
(
'
hyq_rhleg_rom
'
,
[
'
Support
'
,
'
Lean
'
])
# We also bound the rotations of the torso.
rbprmBuilder
.
boundSO3
([
-
0.4
,
0.4
,
-
3
,
3
,
-
3
,
3
])
# Creating an instance of HPP problem solver and the viewer
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.63
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.63
];
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
)
r
.
loadObstacleModel
(
packageName
,
"
darpa
"
,
"
planning
"
)
from
hpp.corbaserver.affordance
import
Client
c
=
Client
()
c
.
affordance
.
analyseAll
()
objs
=
c
.
affordance
.
getAffordancePoints
(
"
Support
"
)
import
random
count
=
0
for
aff
in
objs
:
colour
=
random
.
random
()
for
tri
in
aff
:
r
.
client
.
gui
.
addTriangleFace
(
'
tri
'
+
str
(
count
),
tri
[
0
],
tri
[
1
],
tri
[
2
],
[
colour
,
1
,
0.5
,
1
])
r
.
client
.
gui
.
addToGroup
(
'
tri
'
+
str
(
count
),
'
planning
'
)
count
+=
1
# Solve the problem
t
=
ps
.
solve
()
# Playing the computed path
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
(
1
)
This diff is collapsed.
Click to expand it.
test2.py
0 → 100644
+
64
−
0
View file @
f369d280
# Importing helper class for setting up a reachability planning problem
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
rootJointType
=
'
freeflyer
'
packageName
=
'
hpp-rbprm-corba
'
meshPackageName
=
'
hpp-rbprm-corba
'
urdfName
=
'
hyq_trunk_large
'
urdfNameRom
=
[
'
hyq_lhleg_rom
'
,
'
hyq_lfleg_rom
'
,
'
hyq_rfleg_rom
'
,
'
hyq_rhleg_rom
'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"
base_joint_xyz
"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
rbprmBuilder
.
setFilter
([
'
hyq_rhleg_rom
'
])
# , 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder
.
setAffordanceFilter
(
'
hyq_rhleg_rom
'
,
[
'
Support
'
,
'
Lean
'
])
# We also bound the rotations of the torso.
rbprmBuilder
.
boundSO3
([
-
0.4
,
0.4
,
-
3
,
3
,
-
3
,
3
])
# Creating an instance of HPP problem solver and the viewer
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
r
.
loadObstacleModel
(
packageName
,
"
darpa
"
,
"
planning
"
)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.63
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.63
];
r
(
q_goal
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
from
hpp.corbaserver.affordance
import
Client
c
=
Client
()
c
.
affordance
.
analyseAll
()
objs
=
c
.
affordance
.
getAffordancePoints
(
"
Support
"
)
import
random
count
=
0
for
aff
in
objs
:
colour
=
random
.
random
()
for
tri
in
aff
:
r
.
client
.
gui
.
addTriangleFace
(
'
tri
'
+
str
(
count
),
tri
[
0
],
tri
[
1
],
tri
[
2
],
[
colour
,
1
,
0.5
,
1
])
r
.
client
.
gui
.
addToGroup
(
'
tri
'
+
str
(
count
),
'
planning
'
)
count
+=
1
# 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
()
# Playing the computed path
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
(
0
)
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment