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
b3f3e91b
Commit
b3f3e91b
authored
Mar 20, 2018
by
Pierre Fernbach
Browse files
[demo] add scripts for talos on flat ground
parent
97d59ab5
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/dynamic/talos/__init__.py
0 → 100644
View file @
b3f3e91b
script/dynamic/talos/flatGround_pyrene_interp.py
0 → 100644
View file @
b3f3e91b
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
time
#from constraint_to_dae import *
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
#from display_tools import *
import
talos.flatGround_pyrene_pathKino
as
tp
import
time
tPlanning
=
tp
.
tPlanning
packageName
=
"talos_description"
meshPackageName
=
"talos_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"talos"
urdfSuffix
=
"_full_v2"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
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"
,
tp
.
aMax
)
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
tp
.
vMax
)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
,
displayCoM
=
True
)
q_ref
=
[
0
,
0
,
1.0192720229567027
,
1
,
0
,
0
,
0
,
# root_joint
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# leg_left
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# leg_right
0
,
0.006761
,
# torso
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0
,
0
,
0.1
,
# arm_left
0
,
0
,
0
,
0
,
0
,
0
,
0
,
# gripper_left
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0
,
0
,
0.1
,
# arm_right
0
,
0
,
0
,
0
,
0
,
0
,
0
,
# gripper_right
0
,
0
,
# head
0
,
0
,
0
,
0
,
0
,
0
];
r
(
q_ref
)
q_init
=
q_ref
[::]
#fullBody.setReferenceConfig(q_ref)
"""
#test correspondance with reduced :
q_init[19] = -0.5
q_init[20] = 0.8
r(q_init)
"""
fullBody
.
setCurrentConfig
(
q_init
)
qfar
=
q_ref
[::]
qfar
[
2
]
=
-
5
tStart
=
time
.
time
()
nbSamples
=
50000
rLegId
=
'rleg'
rLeg
=
'leg_right_1_joint'
rfoot
=
'leg_right_sole_fix_joint'
rLegOffset
=
[
0
,
0
,
0
]
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"forward"
,
0.01
)
#fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
lLegId
=
'lleg'
lLeg
=
'leg_left_1_joint'
lfoot
=
'leg_left_sole_fix_joint'
lLegOffset
=
[
0
,
0
,
0
]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"forward"
,
0.01
)
#fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
"""
rarmId = 'rarm'
rarm = 'arm_right_1_joint'
rHand = 'gripper_right_inner_single_joint'
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,1]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
larmId = 'larm'
larm = 'arm_left_1_joint'
lHand = 'gripper_left_inner_single_joint'
lArmOffset = [0,0,-0.1]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
"""
tGenerate
=
time
.
time
()
-
tStart
print
"generate databases in : "
+
str
(
tGenerate
)
+
" s"
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
basic
.
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
]
+
0.01
q_goal
[
2
]
=
q_ref
[
2
]
+
0.01
fullBody
.
setStaticStability
(
True
)
# Randomly generating a contact configuration at q_init
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
(
'talos/base_link'
,
0.3
)
r
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configsFull
=
fullBody
.
interpolate
(
0.001
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"number of configs :"
,
len
(
configsFull
)
from
planning.config
import
*
from
generate_contact_sequence
import
*
beginState
=
0
endState
=
len
(
configsFull
)
-
2
configs
=
configsFull
[
beginState
:
endState
+
1
]
cs
=
generateContactSequence
(
fullBody
,
configs
,
beginState
,
endState
,
r
,
curves_initGuess
=
curves_initGuess
,
timings_initGuess
=
timings_initGuess
)
#player.displayContactPlan()
filename
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
cs
.
saveAsXML
(
filename
,
"ContactSequence"
)
print
"save contact sequence : "
,
filename
script/dynamic/talos/flatGround_pyrene_pathKino.py
0 → 100644
View file @
b3f3e91b
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
import
omniORB.any
from
planning.configs.straight_walk_config
import
*
import
time
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'talos-rbprm'
meshPackageName
=
'talos-rbprm'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'talos_trunk'
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
=
'talos-rbprm'
meshPackageName
=
'talos-rbprm'
urdfName
=
'talos_trunk'
urdfNameRom
=
[
'talos_larm_rom'
,
'talos_rarm_rom'
,
'talos_lleg_rom'
,
'talos_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
omniORB
.
any
.
to_any
(
0.2
);
aMax
=
omniORB
.
any
.
to_any
(
0.1
);
#aMax = omniORB.any.to_any(0.3);
extraDof
=
6
mu
=
omniORB
.
any
.
to_any
(
MU
)
# 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"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder
.
setFilter
([
'talos_lleg_rom'
,
'talos_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'talos_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'talos_rleg_rom'
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
0.65
,
0.65
,
-
0.2
,
0.2
])
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
-
1
,
1
,
-
1
,
1
,
-
2
,
2
,
0
,
0
,
0
,
0
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver and the viewer
ps
=
ProblemSolver
(
rbprmBuilder
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
aMax
)
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
vMax
)
ps
.
client
.
problem
.
setParameter
(
"sizeFootX"
,
omniORB
.
any
.
to_any
(
0.2
))
ps
.
client
.
problem
.
setParameter
(
"sizeFootY"
,
omniORB
.
any
.
to_any
(
0.12
))
ps
.
client
.
problem
.
setParameter
(
"friction"
,
mu
)
r
=
Viewer
(
ps
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
ENV_PACKAGE_NAME
,
ENV_NAME
,
ENV_PREFIX
,
r
)
#r.loadObstacleModel (packageName, "ground", "planning")
#afftool.visualiseAffordances('Support', r, r.color.lightBrown)
#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
,
0
,
0.98
];
r
(
q_init
)
"""
# test correspondance with fullBody :
q_init[7] = -0.5
q_init[8] = 0.7
r(q_init)
"""
#q_init[3:7] = [0.7071,0,0,0.7071]
#q_init [0:3] = [1, 1, 0.65]
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_goal
[
0
]
=
1.5
r
(
q_goal
)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; 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
(
"RbprmDynamicPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"KinodynamicDistance"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
ps
.
selectPathProjector
(
'Progressive'
,
0.05
)
#solve the problem :
r
(
q_init
)
#r.solveAndDisplay("rm",1,0.01)
tStart
=
time
.
time
()
t
=
ps
.
solve
()
tPlanning
=
time
.
time
()
-
tStart
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
.
dt
=
0.03
pp
.
displayVelocityPath
(
0
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
"""
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "
\n
")
f.close()
"""
"""
for i in range(0,9):
t = ps.solve()
if isinstance(t, list):
ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.
f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
f.write("t = "+str(ts) + "
\n
")
f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "
\n
")
f.close()
print "problem "+str(i)+" solved
\n
"
ps.clearRoadmap()
"""
#ps.client.problem.prepareSolveStepByStep()
#ps.client.problem.finishSolveStepByStep()
"""
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)
"""
"""
r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmstart",r.sceneName)
r.client.gui.removeFromGroup("rmgoal",r.sceneName)
for i in range(0,ps.numberNodes()):
r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
"""
"""
# for seed 1486657707
ps.client.problem.extractPath(0,0,2.15)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
#pp (0)
"""
#display path with post-optimisation
q_far
=
q_init
[::]
q_far
[
2
]
=
-
3
r
(
q_far
)
# Manually add waypoints to roadmap:
"""
ps.client.problem.prepareSolveStepByStep()
pbCl = rbprmBuilder.client.basic.problem
q1= [0.6, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 3, 0.0, -1.5, 0.0, 0.0, 0.0]
pbCl.addConfigToRoadmap (q1)
pbCl.directPath(q1,q_goal,True)
pbCl.directPath(q_init,q1,False)
r.client.gui.removeFromGroup("path_"+str(ps.numberPaths()-2)+"_root",r.sceneName)
pp.displayVelocityPath(ps.numberPaths()-1)
pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
"""
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