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
96a529e3
Commit
96a529e3
authored
Jun 25, 2018
by
Pierre Fernbach
Browse files
[script] add updated script without dependancies to locomote : slalom and darpa
parent
538ca47d
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/dynamic/darpa_hrp2_interp.py
0 → 100644
View file @
96a529e3
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
from
tools
import
*
import
darpa_hrp2_path
as
tp
import
time
import
omniORB.any
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.05
,
0.05
,
0.55
,
0.85
])
fullBody
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
-
0
,
0
,
-
0
,
0
,
-
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
omniORB
.
any
.
to_any
(
tp
.
aMax
))
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
omniORB
.
any
.
to_any
(
tp
.
vMax
))
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
,
displayArrows
=
True
,
displayCoM
=
True
)
q_init
=
[
0
,
0
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0
,
0
,
0
,
0
,
0
,
0
];
r
(
q_init
)
q_ref
=
q_init
[::]
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setReferenceConfig
(
q_ref
)
#~ AFTER loading obstacles
rLegId
=
'hrp2_rleg_rom'
lLegId
=
'hrp2_lleg_rom'
tStart
=
time
.
time
()
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
0
,
-
0.105
]
rLegLimbOffset
=
[
0
,
0
,
-
0.035
]
#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.5
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
0
,
-
0.105
]
lLegLimbOffset
=
[
0
,
0
,
0.035
]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.5
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
## Add arms (not used for contact) :
tGenerate
=
time
.
time
()
-
tStart
print
"generate databases in : "
+
str
(
tGenerate
)
+
" s"
"""
fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
tLoad = time.time() - tStart
print "Load databases in : "+str(tLoad)+" s"
"""
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
eps
=
0.0001
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.0001
)[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
1
# 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
]
# Randomly generating a contact configuration at q_init
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
r
(
q_init
)
q_init
=
fullBody
.
generateContacts
(
q_init
,
dir_init
,
acc_init
,
robTreshold
)
r
(
q_init
)
# Randomly generating a contact configuration at q_end
fullBody
.
setCurrentConfig
(
q_goal
)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
dir_goal
,
acc_goal
,
robTreshold
)
r
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
r
.
addLandmark
(
'hrp2_14/BODY'
,
0.3
)
r
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
fullBody
.
setStaticStability
(
True
)
# only set it after the init/goal configuration are computed
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
)
tInterpolate
=
time
.
time
()
-
tStart
print
"number of configs : "
,
len
(
configs
)
print
"generated in "
+
str
(
tInterpolate
)
+
" s"
r
(
configs
[
len
(
configs
)
-
2
])
from
display_tools
import
*
displayContactSequence
(
r
,
configs
)
"""
from planning.configs.darpa import *
from generate_contact_sequence import *
beginState = 0
endState = len(configs)-1
cs = generateContactSequence(fullBody,configs,beginState, endState,r)
"""
"""
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
print "save contact sequence : ",filename
"""
"""
r(q_init)
pos=fullBody.getJointPosition('RLEG_JOINT0')
addSphere(r,r.color.blue,pos)
dir = fullBody.getCurrentConfig()[37:40]
fullBody.client.rbprm.rbprm.evaluateConfig(fullBody.getCurrentConfig(),dir)
vd[0:3] = fullBody.getCurrentConfig()[0:3]
addVector(r,fullBody,r.color.black,vd)
vl[0:3] = fullBody.getCurrentConfig()[0:3]
addVector(r,fullBody,r.color.blue,vl)
vlb[0:3] = fullBody.getCurrentConfig()[0:3]
addVector(r,fullBody,r.color.red,vlb)
"""
#wid = r.client.gui.getWindowID("window_hpp_")
#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
script/dynamic/darpa_hrp2_path.py
View file @
96a529e3
...
...
@@ -8,7 +8,7 @@ import math
import
omniORB.any
from
configs.darpa
import
*
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
...
...
@@ -54,6 +54,7 @@ rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
vMax
=
0.2
;
aMax
=
0.1
;
extraDof
=
6
MU
=
0.5
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.1
,
0.1
,
0.55
,
0.85
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
-
0.05
,
0.05
])
...
...
script/dynamic/slalom_bauzil_hrp2_interp.py
0 → 100644
View file @
96a529e3
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
#from tools import *
import
slalom_bauzil_hrp2_path
as
tp
import
time
import
omniORB.any
from
display_tools
import
*
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced_safe"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
tPlanning
=
0.
rleg
=
'RLEG_JOINT0'
rfoot
=
"RLEG_JOINT5"
lleg
=
'LLEG_JOINT0'
lfoot
=
"LLEG_JOINT5"
rarm
=
"RARM_JOINT0"
rhand
=
"RARM_JOINT5"
larm
=
"LARM_JOINT0"
lhand
=
"LARM_JOINT5"
rLegId
=
'hrp2_rleg_rom'
lLegId
=
'hrp2_lleg_rom'
rArmId
=
'hrp2_rarm_rom'
lArmId
=
'hrp2_larm_rom'
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2.5
,
0.5
,
3
,
0.4
,
0.8
])
fullBody
.
setJointBounds
(
"LLEG_JOINT3"
,
[
0.35
,
2.61799
])
fullBody
.
setJointBounds
(
"RLEG_JOINT3"
,
[
0.35
,
2.61799
])
fullBody
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
omniORB
.
any
.
to_any
(
tp
.
aMax
))
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
omniORB
.
any
.
to_any
(
tp
.
vMax
))
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
,
displayArrows
=
False
,
displayCoM
=
True
)
q_init
=
[
0
,
0
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0
,
0
,
0
,
0
,
0
,
0
];
r
(
q_init
)
q_ref
=
q_init
[::]
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setReferenceConfig
(
q_init
)
#~ AFTER loading obstacles
tStart
=
time
.
time
()
rLegOffset
=
[
0
,
0
,
-
0.0955
]
rLegLimbOffset
=
[
0
,
0
,
-
0.035
]
#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody
.
addLimb
(
rLegId
,
rleg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.4
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLegOffset
=
[
0
,
0
,
-
0.0955
]
lLegLimbOffset
=
[
0
,
0
,
0.035
]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody
.
addLimb
(
lLegId
,
lleg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.4
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
tGenerate
=
time
.
time
()
-
tStart
print
"generate databases in : "
+
str
(
tGenerate
)
+
" s"
"""
fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
tLoad = time.time() - tStart
print "Load databases in : "+str(tLoad)+" s"
"""
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
eps
=
0.0001
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_ref
[::]
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.0001
)[
0
:
7
]
dir_init
=
[
0
,
0
,
0
]
acc_init
=
[
0
,
0
,
0
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
1
# 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
]
# FIXME : test
q_init
[
2
]
=
q_ref
[
2
]
+
0.0005
q_goal
[
2
]
=
q_ref
[
2
]
+
0.0005
# Randomly generating a contact configuration at q_init
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
r
(
q_init
)
# Randomly generating a contact configuration at q_end
fullBody
.
setCurrentConfig
(
q_goal
)
#q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
r
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
r
.
addLandmark
(
'hrp2_14/BODY'
,
0.3
)
r
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
lLegId
,
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
lLegId
,
rLegId
])
fullBody
.
setStaticStability
(
True
)
# only set it after the init/goal configuration are computed
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"number of configs : "
,
len
(
configs
)
print
"generated in "
+
str
(
tInterpolateConfigs
)
+
" s"
r
(
configs
[
1
])
displayContactSequence
(
r
,
configs
)
script/dynamic/slalom_bauzil_hrp2_path.py
0 → 100644
View file @
96a529e3
## 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
import
time
import
math
import
omniORB.any
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hrp2_trunk_flexible'
urdfSuffix
=
""
srdfSuffix
=
""
def
__init__
(
self
,
robotName
,
load
=
True
):
Parent
.
__init__
(
self
,
robotName
,
self
.
rootJointType
,
load
)
self
.
tf_root
=
"base_footprint"
self
.
client
.
basic
=
Client
()
self
.
load
=
load
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hrp2_trunk_arms_flexible'
rLegId
=
'hrp2_rleg_rom'
lLegId
=
'hrp2_lleg_rom'
rArmId
=
'hrp2_rarm_rom'
lArmId
=
'hrp2_larm_rom'
urdfNameRoms
=
[
rLegId
,
lLegId
,
rArmId
,
lArmId
]
urdfSuffix
=
""
srdfSuffix
=
""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRoms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder
.
setFilter
([
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rleg_rom'
,
[
'Support'
])
vMax
=
0.2
;
aMax
=
0.1
;
extraDof
=
6
MU
=
0.5
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6])
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2.5
,
0.5
,
3
,
0.6
,
0.6
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
-
0.05
,
0.05
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.05
,
0.05
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
math
.
pi
,
math
.
pi
,
-
0.0
,
0.0
,
-
0.0
,
0.0
])
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver and the viewer
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
omniORB
.
any
.
to_any
(
aMax
))
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
omniORB
.
any
.
to_any
(
vMax
))
ps
.
client
.
problem
.
setParameter
(
"orientedPath"
,
omniORB
.
any
.
to_any
(
1.
))
ps
.
client
.
problem
.
setParameter
(
"friction"
,
omniORB
.
any
.
to_any
(
MU
))
ps
.
client
.
problem
.
setParameter
(
"sizeFootX"
,
omniORB
.
any
.
to_any
(
0.24
))
ps
.
client
.
problem
.
setParameter
(
"sizeFootY"
,
omniORB
.
any
.
to_any
(
0.14
))
r
=
Viewer
(
ps
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.2
])
afftool
.
loadObstacleModel
(
'hpp-rbprm-corba'
,
"floor_bauzil"
,
"planning"
,
r
,
reduceSizes
=
[
0.2
,
0
,
0
])
#r.loadObstacleModel (packageName, "ground", "planning")
#afftool.visualiseAffordances('Support', r,r.color.lightYellow)
r
.
addLandmark
(
r
.
sceneName
,
1
)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_init
[
0
:
3
]
=
[
-
0.9
,
1
,
0.6
];
r
(
q_init
)
#q_init [0:3] = [0.5, 2.5, 0.6]; r (q_init)
q_init
[
-
6
:
-
3
]
=
[
0.05
,
0
,
0
]
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
2
,
2.5
,
0.6
];
r
(
q_goal
)
#q_goal [0:3] = [1.83, 0.86, 1.13]; r(q_goal)
# Choosing a path optimizer
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.01
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"KinodynamicDistance"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
ps
.
addPathOptimizer
(
"OrientedPathOptimizer"
)
#solve the problem :
r
(
q_init
)
#r.solveAndDisplay("rm",1,radiusSphere=0.01)
t
=
ps
.
solve
()
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
.
dt
=
0.03
pp
.
displayVelocityPath
(
2
)
r
.
client
.
gui
.
setVisibility
(
"path_2_root"
,
"ALWAYS_ON_TOP"
)
q_far
=
q_init
[::]
q_far
[
2
]
=
-
3
r
(
q_far
)
"""
camera = [0.6293167471885681,
-9.560577392578125,
10.504343032836914,
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
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