Commit ef8f4f7a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

script detour and slalom work with profiling

parent bf69f919
......@@ -29,7 +29,7 @@ SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
SET(PROJECT_URL "")
# Set to 1 for profiling
#add_definitions(-DPROFILE)
add_definitions(-DPROFILE)
SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
......
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.13 0.17 0.24 21.95 130
complete generation 22260.40 22260.40 22260.40 22260.40 1
ik 0.07 0.07 0.10 9.33 130
test balance 0.08 0.12 0.18 15.72 130
contact: 135
planner succeeded: 1
unstable contact: 3
path computation 34826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.18 0.37 30.36 173
complete generation 44650.94 44650.94 44650.94 44650.94 1
ik 0.07 0.07 0.13 12.86 173
test balance 0.08 0.13 0.24 23.06 173
contact: 178
planner succeeded: 1
unstable contact: 3
path computation 64826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.05 0.17 0.24 25.03 151
complete generation 49351.72 49351.72 49351.72 49351.72 1
ik 0.06 0.07 0.09 10.81 151
test balance 0.09 0.13 0.19 19.05 142
contact: 147
planner succeeded: 1
unstable contact: 3
path computation 9826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.25 23.45 137
complete generation 20112.68 20112.68 20112.68 20112.68 1
ik 0.07 0.07 0.14 10.06 137
test balance 0.08 0.13 0.21 17.87 137
contact: 140
planner succeeded: 1
unstable contact: 5
path computation 44826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.18 0.30 24.60 137
complete generation 26778.59 26778.59 26778.59 26778.59 1
ik 0.06 0.08 0.15 10.58 137
test balance 0.08 0.14 0.24 18.82 137
contact: 142
planner succeeded: 1
unstable contact: 3
path computation 24826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.15 0.17 0.22 20.92 124
complete generation 17380.98 17380.98 17380.98 17380.98 1
ik 0.06 0.07 0.09 9.03 124
test balance 0.09 0.14 0.22 17.09 124
contact: 129
planner succeeded: 1
unstable contact: 3
path computation 24826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.27 18.70 110
complete generation 14177.46 14177.46 14177.46 14177.46 1
ik 0.06 0.07 0.12 8.05 110
test balance 0.09 0.14 0.21 15.10 110
contact: 115
planner succeeded: 1
unstable contact: 3
path computation 24826
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.30 19.44 113
complete generation 14536.93 14536.93 14536.93 14536.93 1
ik 0.06 0.07 0.17 8.47 113
test balance 0.09 0.14 0.23 15.47 113
contact: 118
planner succeeded: 1
unstable contact: 3
path computation 24826
contact: 128
planner failed: 1
unstable contact: 3
path computation 147468
contact: 116
planner failed: 1
unstable contact: 3
path computation 51695
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.26 29.79 174
complete generation 27216.90 27216.90 27216.90 27216.90 1
ik 0.06 0.07 0.12 12.88 174
test balance 0.08 0.13 0.21 23.01 174
contact: 179
planner succeeded: 1
unstable contact: 3
path computation 35540
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.27 27.04 160
complete generation 29102.41 29102.41 29102.41 29102.41 1
ik 0.07 0.08 0.12 12.09 160
test balance 0.08 0.13 0.20 20.84 160
contact: 165
planner succeeded: 1
unstable contact: 3
path computation 83144
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.13 0.17 0.29 26.88 155
complete generation 51097.25 51097.25 51097.25 51097.25 1
ik 0.07 0.07 0.13 11.28 155
test balance 0.09 0.13 0.23 20.52 155
contact: 159
planner succeeded: 1
unstable contact: 4
path computation 187300
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.11 0.17 0.30 31.18 179
complete generation 38000.61 38000.61 38000.61 38000.61 1
ik 0.06 0.08 0.14 13.54 179
test balance 0.08 0.14 0.25 24.61 176
contact: 181
planner succeeded: 1
unstable contact: 3
path computation 36312
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.06 0.17 0.28 21.54 128
complete generation 10593.58 10593.58 10593.58 10593.58 1
ik 0.07 0.08 0.12 9.68 128
test balance 0.08 0.13 0.18 16.48 125
contact: 130
planner succeeded: 1
unstable contact: 3
path computation 66472
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.27 20.75 121
complete generation 23098.15 23098.15 23098.15 23098.15 1
ik 0.07 0.08 0.15 9.14 121
test balance 0.08 0.12 0.21 15.00 121
contact: 126
planner succeeded: 1
unstable contact: 3
path computation 79946
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.06 0.17 0.28 21.54 128
complete generation 10593.58 10593.58 10593.58 10593.58 1
ik 0.07 0.08 0.12 9.68 128
test balance 0.08 0.13 0.18 16.48 125
contact: 130
planner succeeded: 1
unstable contact: 3
path computation 76032
*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
collision 0.14 0.17 0.22 21.34 125
complete generation 15167.09 15167.09 15167.09 15167.09 1
ik 0.06 0.07 0.11 9.33 125
test balance 0.09 0.13 0.18 16.27 125
contact: 130
planner succeeded: 1
unstable contact: 3
path computation 80763
......@@ -7,9 +7,9 @@ import datetime
import time
#~ scenarios = ['standing_hrp2']
scenarios = ['ground_crouch_hyq']
scenarios = ['slalom_hyq']
#~ scenarios = ['stair_bauzil_hrp2']
n_trials = 200
n_trials = 20
stats = ['balance','collision','ik']
stats_optim = ['time_cwc','com_traj']
......@@ -206,11 +206,10 @@ def analyzeData():
printOneStatOptim(f, stat, d)
f.write ("\n \n \n")
f.close()
for scenario in scenarios:
#~ for i in range(0,n_trials):
#~ var = scenario+python_script_extension;
#~ sp.check_call(["./profile.sh", str(var)]);
#~ time.sleep(3)
parseData(scenario)
for i in range(0,n_trials):
var = scenario+python_script_extension;
sp.check_call(["./profile.sh", str(var)]);
time.sleep(3)
parseData(scenario)
analyzeData()
......@@ -5,12 +5,13 @@ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
import detour_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
pathId = tp.ps.numberPaths()-1
packageName = "hyq_description"
meshPackageName = "hyq_description"
......@@ -24,99 +25,116 @@ srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.setJointBounds ("base_joint_xyz", [-2.5,2.5, -6, 6, 0.6, 0.65])
# Setting a number of sample configurations used
nbSamples = 20000
dynamic=True
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps)
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
rLegId = 'rfleg'
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
addLimbDb(rLegId, "manipulability")
addLimbDb(lLegId, "manipulability")
addLimbDb(rarmId, "manipulability")
addLimbDb(larmId, "manipulability")
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[0:7]
dir_init = tp.ps.configAtParam(pathId,0.01)[7:10]
acc_init = tp.ps.configAtParam(pathId,0.01)[10:13]
dir_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[7:10]
acc_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[10:13]
configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
# 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] = acc_goal[::]
fullBody.setStaticStability(False)
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
#~ from pickle import load
#~ f = open("config_"+str(tp.config_i), 'r')
#~ q_init = load(f)
#~ f.close()
q_init = fullBody.generateContacts(q_init,dir_init,acc_init,2)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,2)
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init,[larmId,rLegId,rarmId,lLegId])
fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
r(q_init)
# computing the contact sequence
#~ configs = fullBody.interpolate(0.05, 10, 1, True)
configs = fullBody.interpolate(0.08, 10, 10, True)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 10, True)
#~ configs = fullBody.interpolate(0.02, 10, 10, True)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
configs = fullBody.interpolate(0.001,pathId=pathId,robustnessTreshold = 0, filterStates = True)
"""
print "number of configs =", len(configs)
r(configs[-1])
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
import fullBodyPlayer
player = fullBodyPlayer.Player(fullBody,pp,tp,configs,draw=True,optim_effector=True,use_velocity=dynamic,pathId = pathId)
#player.displayContactPlan()
r(configs[2])
player.interpolate(2,40)
"""
#player.play()
"""
camera = [0.5681925415992737,
-6.707448482513428,
2.5206544399261475,
0.8217507600784302,
0.5693002343177795,
0.020600343123078346,
0.01408931240439415]
r.client.gui.setCameraTransform(0,camera)
"""
"""
import hpp.corbaserver.rbprm.tools.cwc_trajectory
import hpp.corbaserver.rbprm.tools.path_to_trajectory
import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
reload(hpp.corbaserver.rbprm.tools.cwc_trajectory)
reload(hpp.corbaserver.rbprm.tools.path_to_trajectory)
reload(hpp.corbaserver.rbprm.tools.cwc_trajectory_helper)
reload(fullBodyPlayer)
"""
# Importing helper class for setting up a reachability planning problem
## 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
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
......@@ -13,11 +14,13 @@ urdfName = 'hyq_trunk_large'
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 2;
aMax = 5;
extraDof = 6
# 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", [-2,5, -1, 1, 0.3, 4])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2.5,2.5, -6, 6, 0.6, 0.65])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
......@@ -25,50 +28,64 @@ 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.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
# We also bound the rotations of the torso. (z, y, x)
#rbprmBuilder.boundSO3([-3,3,-0.1,0.1,-0.1,0.1])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
# 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",aMax)
ps.client.problem.setParameter("vMax",vMax)
r = Viewer (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "detour", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName,1)
# 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] = [-1.8, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init[3:7] = [1,0,0,0]
q_init [0:3] = [2, 2, 0.63]; r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
config_i = 0
#~ from pickle import load
#~ f = open("config_"+str(config_i), 'r')
#~ q_init [0:7] = load(f)[0:7]
#~ f.close()
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[3:7] = [1,0,0,0]
q_goal [0:3] = [1.5, -4.5, 0.63]; r (q_goal)
q_goal[7:10] = [1.8,0,0]
r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
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)
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")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ r.loadObstacleModel (packageName, "darpa", "planning")
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
#solve the problem :
r(q_init)
#ps.client.problem.prepareSolveStepByStep()
q_far = q_init[::]
q_far[2] = -3
r(q_far)
#r.solveAndDisplay("rm",1,0.01)
# Solve the problem
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
......@@ -76,14 +93,80 @@ f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (1)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
pp.dt=0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.5
#pp (0)
#display path with post-optimisation
"""
ps.client.problem.extractPath(0,0,11.18)
r.client.gui.removeFromGroup("path_0_root",r.sceneName)
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#pp (1)
"""
"""
q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
"""
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
rbprmBuilder.client.basic.problem.optimizePath(i)
r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
pp.displayVelocityPath(i+1)
#time.sleep(2)
"""
"""
i=0
ps.clearRoadmap()
ps.solve()
r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
i = i+1
pp.displayVelocityPath(i)
pp(i)
"""
"""
r.client.gui.addCurve("c1",qlist,r.color.red)
r.client.gui.setVisibility("c1","ALWAYS_ON_TOP")
r.client.gui.addToGroup("c1",r.sceneName)
r.client.gui.addCurve("c2",qlist2,r.color.blue)
r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
r.client.gui.addToGroup("c2",r.sceneName)
"""
"""
nodes = ["hyq_trunk_large/base_link","Vec_Acceleration","Vec_Velocity"]
r.client.gui.setCaptureTransform("yaml/hyq_slalom_path.yaml",nodes)
r.client.gui.captureTransformOnRefresh(True)
pp(0)
r.client.gui.captureTransformOnRefresh(False)
r.client.gui.writeNodeFile('path_0_root','/local/dev_hpp/screenBlender/iros2017/meshs/detour_path_kino.obj')
"""
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import ground_crouch_hyq_path as tp
packageName = "hyq_description"