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
Jason Chemin
hpp-rbprm-corba
Commits
b617fef4
Commit
b617fef4
authored
Jan 27, 2019
by
Pierre Fernbach
Browse files
[script] use hyq_contact6D in darpa (required for whole body motion)
parent
79a2dcfb
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/darpa_hyq.py
View file @
b617fef4
#Importing helper class for RBPRM
from
hpp.corbaserver.rbprm.hyq
import
Robot
from
hpp.corbaserver.rbprm.hyq
_contact6D
import
Robot
from
hpp.corbaserver.problem_solver
import
ProblemSolver
from
hpp.gepetto
import
Viewer
...
...
@@ -23,10 +23,10 @@ fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
nbSamples
=
10000
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
)
rootName
=
'base_joint_xyz'
cType
=
"_
3
_DOF"
cType
=
"_
6
_DOF"
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
...
...
@@ -61,8 +61,8 @@ q_init = [-2.0,
-0.9577096766517215,
0.9384602821326071]
"""
q_init
=
fullBody
.
referenceConfig
[::];
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
];
q_init
[
2
]
=
fullBody
.
referenceConfig
[
2
]
+
0.02
q_goal
=
fullBody
.
referenceConfig
[::];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_goal
[
2
]
=
fullBody
.
referenceConfig
[
2
]
+
0.02
q_init
=
fullBody
.
referenceConfig
[::];
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
];
q_init
[
2
]
=
fullBody
.
referenceConfig
[
2
]
q_goal
=
fullBody
.
referenceConfig
[::];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_goal
[
2
]
=
fullBody
.
referenceConfig
[
2
]
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
...
...
@@ -73,12 +73,12 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
])
r
(
q_init
)
v
(
q_init
)
configs
=
[]
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
,
r
)
pp
=
PathPlayer
(
fullBody
.
client
,
v
)
import
time
...
...
@@ -86,36 +86,36 @@ import time
#DEMO METHODS
def
initConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
r
(
q_init
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
r
(
q_goal
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
v
(
q_goal
)
def
rootPath
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"ON"
)
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
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
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"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
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
.
clock
()
configs
=
fullBody
.
interpolate
(
stepsize
,
8
,
0
,
filterStates
=
False
,
testReachability
=
False
,
quasiStatic
=
True
)
...
...
@@ -123,12 +123,12 @@ def genPlan(stepsize=0.06):
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
(
step
=
0.5
):
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
v
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
v
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)):
r
(
configs
[
i
]);
v
(
configs
[
i
]);
time
.
sleep
(
step
)
...
...
script/scenarios/demos/darpa_hyq_path.py
View file @
b617fef4
...
...
@@ -21,13 +21,24 @@ 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
)
r
=
Viewer
(
ps
)
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-rbprm-corba"
,
"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.63
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.63
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
v
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.63
];
r
(
q_goal
)
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.63
];
v
(
q_goal
)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
...
...
@@ -35,12 +46,6 @@ ps.addPathOptimizer("RandomShortcut")
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool
.
loadObstacleModel
(
"hpp-rbprm-corba"
,
"darpa"
,
"planning"
,
r
)
afftool
.
visualiseAffordances
(
'Support'
,
r
,
[
0.25
,
0.5
,
0.5
])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps
.
client
.
problem
.
selectConfigurationShooter
(
"RbprmShooter"
)
...
...
@@ -56,11 +61,11 @@ print "computation time for root path ", t
# Playing the computed path
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
r
)
pp
=
PathPlayer
(
v
)
q_far
=
q_init
[::]
q_far
[
0
:
3
]
=
[
-
2
,
-
3
,
0.63
];
r
(
q_far
)
v
(
q_far
)
for
i
in
range
(
1
,
10
):
rbprmBuilder
.
client
.
problem
.
optimizePath
(
i
)
...
...
@@ -76,6 +81,6 @@ rbprmBuilder2 = Robot ("toto")
ps2
=
ProblemSolver
(
rbprmBuilder2
)
cl
.
problem
.
selectProblem
(
"default"
)
cl
.
problem
.
movePathToProblem
(
8
,
"rbprm_path"
,
rbprmBuilder
.
getAllJointNames
()[
1
:])
r2
=
Viewer
(
ps2
,
viewerClient
=
r
.
client
)
r2
=
Viewer
(
ps2
,
viewerClient
=
v
.
client
)
r2
(
q_far
)
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