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
523b464e
Commit
523b464e
authored
Jan 18, 2019
by
Pierre Fernbach
Browse files
[script] update scripts to change of robot-data location
parent
f7829c9b
Changes
9
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/darpa_hyq.py
View file @
523b464e
#Importing helper class for RBPRM
from
hpp.corbaserver.rbprm.
rbprmfullbody
import
FullBody
from
hpp.corbaserver.rbprm.
hyq
import
Robot
from
hpp.corbaserver.problem_solver
import
ProblemSolver
from
hpp.gepetto
import
Viewer
#reference pose for hyq
from
hyq_ref_pose
import
hyq_ref
#calling script darpa_hyq_path to compute root path
import
darpa_hyq_path
as
tp
...
...
@@ -16,18 +14,9 @@ db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from
hpp.corbaserver
import
Client
packageName
=
"hyq_description"
meshPackageName
=
"hyq_description"
rootJointType
=
"freeflyer"
# Information to retrieve urdf and srdf files.
urdfName
=
"hyq"
urdfSuffix
=
""
srdfSuffix
=
""
# This time we load the full body model of HyQ
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
=
Robot
()
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
# Setting a number of sample configurations used
...
...
@@ -37,40 +26,21 @@ ps = tp.ProblemSolver(fullBody)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
)
rootName
=
'base_joint_xyz'
cType
=
"_3_DOF"
rLegId
=
'rfleg'
rLeg
=
'rf_haa_joint'
rfoot
=
'rf_foot_joint'
offset
=
[
0.
,
-
0.021
,
0.
]
normal
=
[
0
,
1
,
0
]
legx
=
0.02
;
legy
=
0.02
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"random"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"random"
,
0.05
,
cType
)
#~
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"random"
,
0.05
,
cType
)
larmId
=
'lfleg'
larm
=
'lf_haa_joint'
lHand
=
'lf_foot_joint'
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"random"
,
0.05
,
cType
)
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
.
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,
...
...
@@ -90,14 +60,15 @@ q_init = [-2.0,
0.03995660287873871,
-0.9577096766517215,
0.9384602821326071]
q_goal
=
hyq_ref
[:];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_goal
[
2
]
=
hyq_ref
[
2
]
+
0.02
"""
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
# specifying the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
,
rarmId
,
la
rmId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
la
rmId
])
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
,
fullBody
.
lA
rmId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
,
fullBody
.
rArmId
,
fullBody
.
lA
rmId
])
r
(
q_init
)
...
...
script/scenarios/demos/darpa_hyq_path.py
View file @
523b464e
# Importing helper class for setting up a reachability planning problem
from
hpp.corbaserver.rbprm.
rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.
hyq_abstract
import
Robot
# 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
=
Robot
()
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
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 ...
...
...
@@ -48,7 +38,7 @@ 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
(
packageName
,
"darpa"
,
"planning"
,
r
)
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.
...
...
@@ -79,22 +69,6 @@ for i in range(1,10):
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'hyq_trunk_large'
urdfSuffix
=
""
srdfSuffix
=
""
def
__init__
(
self
,
robotName
,
load
=
True
):
Parent
.
__init__
(
self
,
robotName
,
self
.
rootJointType
,
load
)
self
.
tf_root
=
"base_footprint"
self
.
client
=
Client
()
self
.
load
=
load
#DEMO code to play root path and final contact plan
cl
=
Client
()
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
...
...
script/scenarios/demos/hyq_ref_pose.py
deleted
100644 → 0
View file @
f7829c9b
hyq_ref
=
[
-
1.652528468457493
,
0.06758953014152885
,
0.6638277139631803
,
0.9995752585582991
,
-
0.016391515572502728
,
-
0.011295242081121508
,
0.02128469407050025
,
0.17905666752078864
,
0.9253512562075908
,
-
0.8776870832724601
,
0.11147422537786231
,
-
0.15843632504615043
,
1.150049183494211
,
-
0.1704998924604114
,
0.6859376445755911
,
-
1.1831277202117043
,
0.06262698472369518
,
-
0.42708925470675
,
1.2855999319965081
]
script/scenarios/demos/talos_flatGround.py
View file @
523b464e
from
hpp.corbaserver.rbprm.
rbprmfullbody
import
FullBody
from
hpp.corbaserver.rbprm.
talos
import
Robot
from
hpp.gepetto
import
Viewer
from
display_tools
import
*
import
time
...
...
@@ -8,37 +8,10 @@ import talos_flatGround_path as tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
rLegId
=
'talos_rleg_rom'
rleg
=
'leg_right_1_joint'
rfoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lleg
=
'leg_left_1_joint'
lfoot
=
'leg_left_6_joint'
rArmId
=
'talos_rarm_rom'
rarm
=
'arm_right_1_joint'
rhand
=
'arm_right_7_joint'
lArmId
=
'talos_larm_rom'
larm
=
'arm_left_1_joint'
lhand
=
'arm_left_7_joint'
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
=
Robot
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
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
])
...
...
@@ -48,15 +21,7 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
q_ref
=
[
0.0
,
0.0
,
1.0232773
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
,
#Head
0
,
0
,
0
,
0
,
0
,
0
];
v
(
q_ref
)
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
...
...
@@ -69,20 +34,12 @@ tStart = time.time()
# generate databases :
nbSamples
=
10000
rLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rleg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lleg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
...
...
@@ -125,8 +82,8 @@ v(q_goal)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
from
hpp.gepetto
import
PathPlayer
...
...
@@ -134,7 +91,7 @@ pp = PathPlayer ( v)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
testReachability
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"number of configs :"
,
len
(
configs
)
...
...
script/scenarios/demos/talos_flatGround_path.py
View file @
523b464e
from
hpp.corbaserver.rbprm.
rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.
talos_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver
import
ProblemSolver
...
...
@@ -6,21 +6,13 @@ import time
rootJointType
=
'freeflyer'
packageName
=
'talos-rbprm'
meshPackageName
=
'talos-rbprm'
urdfName
=
'talos_trunk'
urdfNameRom
=
[
'talos_larm_rom'
,
'talos_rarm_rom'
,
'talos_lleg_rom'
,
'talos_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
0.3
aMax
=
0.1
extraDof
=
6
mu
=
0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
=
Robot
()
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
...
...
@@ -45,14 +37,6 @@ ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
p_lLeg
=
[
-
0.008846952891378526
,
0.0848172440888579
,
-
1.019272022956703
]
p_lLeg
[
0
]
=
0.
# assure symetry of dynamic constraints on flat ground
p_rLeg
=
[
-
0.008846952891378526
,
-
0.0848172440888579
,
-
1.019272022956703
]
p_rLeg
[
0
]
=
0.
p_lArm
=
[
0.13028765672452458
,
0.44360498616312666
,
-
0.2881211563246389
]
p_rArm
=
[
0.13028765672452458
,
-
0.44360498616312666
,
-
0.2881211563246389
]
rbprmBuilder
.
setReferenceEndEffector
(
'talos_lleg_rom'
,
p_lLeg
)
rbprmBuilder
.
setReferenceEndEffector
(
'talos_rleg_rom'
,
p_rLeg
)
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
...
...
@@ -62,7 +46,7 @@ afftool = AffordanceTool ()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp-rbprm-corba"
,
"ground"
,
"planning"
,
vf
)
v
=
vf
.
createViewer
(
displayArrows
=
True
)
afftool
.
visualiseAffordances
(
'Support'
,
v
,
v
.
color
.
lightBrown
)
#
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init
=
rbprmBuilder
.
getCurrentConfig
();
...
...
script/scenarios/demos/talos_navBauzil.py
View file @
523b464e
from
hpp.corbaserver.rbprm.
rbprmfullbody
import
FullBody
from
hpp.corbaserver.rbprm.
talos
import
Robot
from
hpp.gepetto
import
Viewer
from
display_tools
import
*
import
time
...
...
@@ -8,37 +8,11 @@ import talos_navBauzil_path as tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
rLegId
=
'talos_rleg_rom'
rleg
=
'leg_right_1_joint'
rfoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lleg
=
'leg_left_1_joint'
lfoot
=
'leg_left_6_joint'
rArmId
=
'talos_rarm_rom'
rarm
=
'arm_right_1_joint'
rhand
=
'arm_right_7_joint'
lArmId
=
'talos_larm_rom'
larm
=
'arm_left_1_joint'
lhand
=
'arm_left_7_joint'
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
=
Robot
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
root_bounds
=
tp
.
root_bounds
[::]
root_bounds
[
0
]
-=
0.2
root_bounds
[
1
]
+=
0.2
...
...
@@ -55,18 +29,9 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
q_ref
=
[
0.0
,
0.0
,
1.0232773
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
,
#Head
0
,
0
,
0
,
0
,
0
,
0
];
v
(
q_ref
)
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
,
0
,
0
,
0
,
0
,
0
]
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
...
...
@@ -76,20 +41,12 @@ tStart = time.time()
# generate databases :
nbSamples
=
10000
rLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rleg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lleg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
...
...
@@ -132,8 +89,8 @@ v(q_goal)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
lLegId
,
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
lLegId
,
rLegId
])
fullBody
.
setStartState
(
q_init
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
from
hpp.gepetto
import
PathPlayer
...
...
@@ -141,7 +98,7 @@ pp = PathPlayer ( v)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
testReachability
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"number of configs :"
,
len
(
configs
)
...
...
script/scenarios/demos/talos_navBauzil_hard.py
0 → 100644
View file @
523b464e
from
hpp.corbaserver.rbprm.talos
import
Robot
from
hpp.gepetto
import
Viewer
from
display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_navBauzil_hard_path
as
tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
root_bounds
=
tp
.
root_bounds
[::]
root_bounds
[
0
]
-=
0.2
root_bounds
[
1
]
+=
0.2
root_bounds
[
2
]
-=
0.2
root_bounds
[
3
]
+=
0.2
root_bounds
[
-
1
]
=
1.2
root_bounds
[
-
2
]
=
0.8
fullBody
.
setJointBounds
(
"root_joint"
,
root_bounds
)
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
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
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
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
q_0
=
fullBody
.
getCurrentConfig
();
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
]
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"number of configs :"
,
len
(
configs
)
raw_input
(
"Press Enter to display the contact sequence ..."
)
displayContactSequence
(
v
,
configs
)
script/scenarios/demos/talos_navBauzil_hard_path.py
View file @
523b464e
from
hpp.corbaserver.rbprm.
rbprmbuilder
import
Builder
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
rootJointType
=
'freeflyer'
packageName
=
'talos-rbprm'
meshPackageName
=
'talos-rbprm'
urdfName
=
'talos_trunk'
urdfNameRom
=
[
'talos_larm_rom'
,
'talos_rarm_rom'
,
'talos_lleg_rom'
,
'talos_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
0.3
aMax
=
0.1
extraDof
=
6
mu
=
0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
=
Robot
()
root_bounds
=
[
-
2.3
,
4.6
,
-
1.5
,
3.3
,
0.98
,
0.98
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
rbprmBuilder
.
setJointBounds
(
'torso_1_joint'
,
[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'torso_2_joint'
,
[
0
,
0
])
#
rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
#
rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
...
...
@@ -47,17 +37,8 @@ ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
ps
.
setParameter
(
"ConfigurationShooter/sampl