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
2a578be9
Unverified
Commit
2a578be9
authored
Feb 07, 2019
by
stonneau
Committed by
GitHub
Feb 07, 2019
Browse files
Merge pull request #19 from pFernbach/devel
Devel
parents
c61085c9
fd6eac27
Changes
12
Hide whitespace changes
Inline
Side-by-side
data/meshes/slalom_debris.stl
0 → 100644
View file @
2a578be9
File added
data/srdf/slalom_debris.srdf
0 → 100644
View file @
2a578be9
<robot name="chair">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
</collision>
</link>
</robot>
data/urdf/slalom_debris.urdf
0 → 100755
View file @
2a578be9
<robot name="chair">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
</collision>
</link>
</robot>
script/scenarios/demos/darpa_hyq.py
View file @
2a578be9
#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
...
...
@@ -18,51 +18,35 @@ 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
)
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
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"
random
"
,
0.1
,
cType
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"
random
"
,
0.05
,
cType
)
fullBody
.
addLimb
(
fullBody
.
rArmId
,
fullBody
.
rarm
,
fullBody
.
rhand
,
fullBody
.
offset
,
fullBody
.
normal
,
fullBody
.
legx
,
fullBody
.
legy
,
nbSamples
,
"
random
"
,
0.05
,
cType
)
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 = [-2.0,
0.0,
0.6838277139631803,
0.0,
0.0,
0.0,
1.0,
0.14279812395541294,
0.934392553166556,
-0.9968239786882757,
-0.06521258938340457,
-0.8831796268418511,
1.150049183494211,
-0.06927610020154493,
0.9507443168724581,
-0.8739975339028809,
0.03995660287873871,
-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
[::]
+
[
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
])
...
...
@@ -73,12 +57,13 @@ 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,49 +71,49 @@ 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
=
Fals
e
,
testReachability
=
False
,
quasiStatic
=
True
)
end
=
time
.
clock
()
start
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
stepsize
,
5
,
0
.
,
filterStates
=
Tru
e
,
testReachability
=
False
)
end
=
time
.
time
()
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 @
2a578be9
...
...
@@ -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.6
3
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.6
4
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
v
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.6
3
];
r
(
q_goal
)
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.6
4
];
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.6
3
];
r
(
q_far
)
q_far
[
0
:
3
]
=
[
-
2
,
-
3
,
0.6
];
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
)
script/scenarios/demos/hyq_slalom_debris.py
0 → 100644
View file @
2a578be9
from
hpp.corbaserver.rbprm.hyq_contact6D
import
Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
import
hyq_slalom_debris_path
as
tp
print
"Done."
import
time
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
# 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
)
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
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)
script/scenarios/demos/hyq_slalom_debris_path.py
0 → 100644
View file @
2a578be9
from
hpp.corbaserver.rbprm.hyq_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
time
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
()
# 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/forceOrientation"
,
True
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
# load the module to analyse the environnement and compute the possible contact surfaces
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp-rbprm-corba"
,
"slalom_debris"
,
"planning"
,
vf
,
reduceSizes
=
[
0.05
,
0
,
0
])
v
=
vf
.
createViewer
(
displayArrows
=
True
)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
1.5
,
0
,
0.63
]
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
-
6
]
=
0.05
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
2.2
v
(
q_goal
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
ps
.
optimizePath
(
1
)
pid
=
ps
.
numberPaths
()
-
1
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
#pp.displayVelocityPath(pid)
#v.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
script/scenarios/demos/talos_flatGround.py
View file @
2a578be9
...
...
@@ -32,9 +32,9 @@ tStart = time.time()
# generate databases :
nbSamples
=
50000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep
1
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep
08
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep
1
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep
08
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
...
...
@@ -84,9 +84,10 @@ tStart = time.time()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
)
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
)
#
raw_input("Press Enter to display the contact sequence ...")
#
displayContactSequence(v,configs)
script/scenarios/demos/talos_flatGround_path.py
View file @
2a578be9
from
hpp.corbaserver.rbprm.talos_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver
import
ProblemSolver
import
time
...
...
@@ -84,9 +83,10 @@ print "Guide planning time : ",t
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.
03
pp
.
displayVelocityPath
(
0
)
pp
.
dt
=
0.
1
#
pp.displayVelocityPath(0)
v
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
.
dt
=
0.01
#pp(0)
# move the robot out of the view before computing the contacts
...
...
script/tools/disp_bezier.py
View file @
2a578be9
from
spline
import
bezier
,
bezier6
,
polynom
,
exact_cubic
,
curve_constraints
,
spline_deriv_constraint
,
from_bezier
from
hpp_
spline
import
bezier
,
bezier6
,
polynom
,
exact_cubic
,
curve_constraints
,
spline_deriv_constraint
,
from_bezier
from
numpy
import
matrix
from
numpy.linalg
import
norm
...
...
script/tools/polyBezier.py
deleted
100644 → 0
View file @
c61085c9
from
spline
import
bezier
,
bezier6
,
polynom
,
exact_cubic
,
curve_constraints
,
spline_deriv_constraint
,
from_bezier
import
inspect
class
PolyBezier
:
def
__init__
(
self
,
curves
):
if
not
isinstance
(
curves
,
list
):
# deal with single bezier curve input
curves
=
[
curves
]
self
.
curves
=
curves
self
.
times
=
[]
self
.
times
+=
[
0
]
self
.
d_curves
=
[]
self
.
dd_curves
=
[]
self
.
jerk_curves
=
[]
for
i
in
range
(
len
(
curves
)):
if
not
isinstance
(
curves
[
i
],
bezier
):
raise
TypeError
(
"PolyBezier must be called with a list of bezier curves (or a single bezier curve)"
)
self
.
times
+=
[
curves
[
i
].
max
()
+
self
.
times
[
-
1
]]
def
findInterval
(
self
,
t
):
if
t
>
self
.
times
[
-
1
]
or
t
<
0
:
raise
ValueError
(
"Parameter is outside of definition range of the curves, t = "
+
str
(
t
)
)
for
cit
in
range
(
len
(
self
.
times
)):
if
t
<=
self
.
times
[
cit
+
1
]:
return
cit
raise
ValueError
(
"Error in times intervals for t = "
+
str
(
t
))
def
findIntervalAdjustTime
(
self
,
t
):
id
=
self
.
findInterval
(
t
)
t
-=
self
.
times
[
id
]
return
id
,
t
def
getBezierAt
(
self
,
t
):
id
=
self
.
findInterval
(
t
)
return
self
.
curves
[
id
]
def
__call__
(
self
,
t
):
id
=
self
.
findInterval
(
t
)
tc
=
t
-
self
.
times
[
id
]
return
self
.
curves
[
id
](
tc
)
def
numCurves
(
self
):
return
len
(
self
.
curves
)
def
length
(
self
):
return
self
.
times
[
-
1
]
def
max
(
self
):
return
self
.
length
()
def
lengthNonZero
(
self
):
length
=
0
for
c
in
self
.
curves
:
if
c
.
degree
>
0
:
length
+=
(
c
.
max
()
-
c
.
min
())
return
length
def
isInFirst
(
self
,
t
):
id
=
self
.
findInterval
(
t
)