Unverified Commit 0bcd5c3f authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #61 from pFernbach/devel

Major refactoring of scripts organization
parents 0bfb5ce4 38c474f9
Subproject commit 2de34adfce816937d2403cb602261ade0c26f0cd Subproject commit 7eca9ee6c9d1c4ee20eb82272e94f9d11642053a
...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot ...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import * from hpp.corbaserver.rbprm.tools.display_tools import *
import time import time
print "Plan guide trajectory ..." print("Plan guide trajectory ...")
import anymal_flatGround_path as tp import anymal_flatGround_path as tp
print "Done." print("Done.")
import time import time
...@@ -39,14 +39,14 @@ fullBody.setCurrentConfig (q_init) ...@@ -39,14 +39,14 @@ fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
#fullBody.usePosturalTaskContactCreation(True) #fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..." print("Generate limb DB ...")
tStart = time.time() tStart = time.time()
dict_heuristic = {fullBody.rLegId:"fixedStep04", fullBody.lLegId:"fixedStep04", fullBody.rArmId:"static", fullBody.lArmId:"static"} dict_heuristic = {fullBody.rLegId:"fixedStep04", fullBody.lLegId:"fixedStep04", fullBody.rArmId:"static", fullBody.lArmId:"static"}
#fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=50000,disableEffectorCollision=False) #fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=50000,disableEffectorCollision=False)
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False) fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart tGenerate = time.time() - tStart
print "Done." print("Done.")
print "Databases generated in : "+str(tGenerate)+" s" print("Databases generated in : "+str(tGenerate)+" s")
fullBody.setReferenceConfig(q_ref) fullBody.setReferenceConfig(q_ref)
#define initial and final configurations : #define initial and final configurations :
...@@ -84,13 +84,13 @@ v(q_goal) ...@@ -84,13 +84,13 @@ v(q_goal)
v.addLandmark('anymal/base',0.3) v.addLandmark('anymal/base',0.3)
print "Generate contact plan ..." print("Generate contact plan ...")
tStart = time.time() tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True) configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart tInterpolateConfigs = time.time() - tStart
print "Done." print("Done.")
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s" print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print "number of configs :", len(configs) print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...") #raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs) #displayContactSequence(v,configs)
......
...@@ -84,7 +84,7 @@ ps.selectPathPlanner("DynamicPlanner") ...@@ -84,7 +84,7 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem : # Solve the planning problem :
t = ps.solve () t = ps.solve ()
print "Guide planning time : ",t print("Guide planning time : ",t)
# display solution : # display solution :
......
...@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot ...@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
from tools.display_tools import * from tools.display_tools import *
import time import time
print "Plan guide trajectory ..." print("Plan guide trajectory ...")
import scenarios.sandbox.ANYmal.anymal_modular_palet_flat_path as tp import scenarios.sandbox.ANYmal.anymal_modular_palet_flat_path as tp
#Robot.urdfSuffix += "_safeFeet" #Robot.urdfSuffix += "_safeFeet"
...@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename ...@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename
pId = tp.pid pId = tp.pid
f = open(statusFilename,"a") f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 : if tp.ps.numberPaths() > 0 :
print "Path planning OK." print("Path planning OK.")
f.write("Planning_success: True"+"\n") f.write("Planning_success: True"+"\n")
f.close() f.close()
else : else :
print "Error during path planning" print("Error during path planning")
f.write("Planning_success: False"+"\n") f.write("Planning_success: False"+"\n")
f.close() f.close()
import sys import sys
...@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu) ...@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu)
try : try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception: except Exception:
print "No viewer started !" print("No viewer started !")
class FakeViewer(): class FakeViewer():
def __init__(self): def __init__(self):
return return
...@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) ...@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.setCurrentConfig (q_init) fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..." print("Generate limb DB ...")
tStart = time.time() tStart = time.time()
# generate databases : # generate databases :
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False) fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart tGenerate = time.time() - tStart
print "Done." print("Done.")
print "Databases generated in : "+str(tGenerate)+" s" print("Databases generated in : "+str(tGenerate)+" s")
v.addLandmark('anymal/base_0',0.2) v.addLandmark('anymal/base_0',0.2)
#define initial and final configurations : #define initial and final configurations :
...@@ -108,27 +108,27 @@ fullBody.setCurrentConfig (q_goal) ...@@ -108,27 +108,27 @@ fullBody.setCurrentConfig (q_goal)
v(q_goal) v(q_goal)
print "Generate contact plan ..." print("Generate contact plan ...")
tStart = time.time() tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True) configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart tInterpolateConfigs = time.time() - tStart
print "Done." print("Done.")
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s" print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print "number of configs :", len(configs) print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...") #raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs) #displayContactSequence(v,configs)
if len(configs) < 2 : if len(configs) < 2 :
cg_success = False cg_success = False
print "Error during contact generation." print("Error during contact generation.")
else: else:
cg_success = True cg_success = True
print "Contact generation Done." print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1): if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print "Contact generation successful." print("Contact generation successful.")
cg_reach_goal = True cg_reach_goal = True
else: else:
print "Contact generation failed to reach the goal." print("Contact generation failed to reach the goal.")
cg_reach_goal = False cg_reach_goal = False
f = open(statusFilename,"a") f = open(statusFilename,"a")
......
...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planni ...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planni
try : try :
v = vf.createViewer(displayArrows = True) v = vf.createViewer(displayArrows = True)
except Exception: except Exception:
print "No viewer started !" print("No viewer started !")
class FakeViewer(): class FakeViewer():
def __init__(self): def __init__(self):
return return
...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal) ...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1 pidBegin = ps.numberPaths()-1
...@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal) ...@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidMiddle = ps.numberPaths()-1 pidMiddle = ps.numberPaths()-1
...@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal) ...@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1 pidLast = ps.numberPaths()-1
......
...@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot ...@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import * from hpp.corbaserver.rbprm.tools.display_tools import *
import time import time
print "Plan guide trajectory ..." print("Plan guide trajectory ...")
import scenarios.sandbox.ANYmal.anymal_modular_palet_low_obstacles_path as tp import scenarios.sandbox.ANYmal.anymal_modular_palet_low_obstacles_path as tp
#Robot.urdfSuffix += "_safeFeet" #Robot.urdfSuffix += "_safeFeet"
...@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename ...@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename
pId = tp.pid pId = tp.pid
f = open(statusFilename,"a") f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 : if tp.ps.numberPaths() > 0 :
print "Path planning OK." print("Path planning OK.")
f.write("Planning_success: True"+"\n") f.write("Planning_success: True"+"\n")
f.close() f.close()
else : else :
print "Error during path planning" print("Error during path planning")
f.write("Planning_success: False"+"\n") f.write("Planning_success: False"+"\n")
f.close() f.close()
import sys import sys
...@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu) ...@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu)
try : try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception: except Exception:
print "No viewer started !" print("No viewer started !")
class FakeViewer(): class FakeViewer():
def __init__(self): def __init__(self):
return return
...@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) ...@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.setCurrentConfig (q_init) fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..." print("Generate limb DB ...")
tStart = time.time() tStart = time.time()
# generate databases : # generate databases :
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False) fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart tGenerate = time.time() - tStart
print "Done." print("Done.")
print "Databases generated in : "+str(tGenerate)+" s" print("Databases generated in : "+str(tGenerate)+" s")
v.addLandmark('anymal/base_0',0.2) v.addLandmark('anymal/base_0',0.2)
#define initial and final configurations : #define initial and final configurations :
...@@ -107,27 +107,27 @@ fullBody.setCurrentConfig (q_goal) ...@@ -107,27 +107,27 @@ fullBody.setCurrentConfig (q_goal)
v(q_goal) v(q_goal)
print "Generate contact plan ..." print("Generate contact plan ...")
tStart = time.time() tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True) configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart tInterpolateConfigs = time.time() - tStart
print "Done." print("Done.")
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s" print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print "number of configs :", len(configs) print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...") #raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs) #displayContactSequence(v,configs)
if len(configs) < 2 : if len(configs) < 2 :
cg_success = False cg_success = False
print "Error during contact generation." print("Error during contact generation.")
else: else:
cg_success = True cg_success = True
print "Contact generation Done." print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1): if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print "Contact generation successful." print("Contact generation successful.")
cg_reach_goal = True cg_reach_goal = True
else: else:
print "Contact generation failed to reach the goal." print("Contact generation failed to reach the goal.")
cg_reach_goal = False cg_reach_goal = False
f = open(statusFilename,"a") f = open(statusFilename,"a")
......
...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_obstacles" ...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_obstacles"
try : try :
v = vf.createViewer(displayArrows = True) v = vf.createViewer(displayArrows = True)
except Exception: except Exception:
print "No viewer started !" print("No viewer started !")
class FakeViewer(): class FakeViewer():
def __init__(self): def __init__(self):
return return
...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal) ...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1 pidBegin = ps.numberPaths()-1
...@@ -126,7 +126,7 @@ ps.addGoalConfig (q_goal) ...@@ -126,7 +126,7 @@ ps.addGoalConfig (q_goal)
rbprmBuilder.setJointBounds ("root_joint", [q_init[0],q_goal[0],-0.2,0.2,q_init[2],q_init[2]]) rbprmBuilder.setJointBounds ("root_joint", [q_init[0],q_goal[0],-0.2,0.2,q_init[2],q_init[2]])
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
#v.solveAndDisplay('rm',2,0.001) #v.solveAndDisplay('rm',2,0.001)
pidMiddle = ps.numberPaths()-1 pidMiddle = ps.numberPaths()-1
rbprmBuilder.setJointBounds ("root_joint", rootBounds) rbprmBuilder.setJointBounds ("root_joint", rootBounds)
...@@ -144,7 +144,7 @@ ps.addGoalConfig (q_goal) ...@@ -144,7 +144,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1 pidLast = ps.numberPaths()-1
......
...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision" ...@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision"
try : try :
v = vf.createViewer(displayArrows = True) v = vf.createViewer(displayArrows = True)
except Exception: except Exception:
print "No viewer started !" print("No viewer started !")
class FakeViewer(): class FakeViewer():
def __init__(self): def __init__(self):
return return
...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal) ...@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1 pidBegin = ps.numberPaths()-1
...@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal) ...@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidMiddle = ps.numberPaths()-1 pidMiddle = ps.numberPaths()-1
...@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal) ...@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem : # Solve the planning problem :
t = ps.solve() t = ps.solve()
print "Guide planning done in "+str(t) print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1 pidLast = ps.numberPaths()-1
......
...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot ...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import * from hpp.corbaserver.rbprm.tools.display_tools import *
import time import time
print "Plan guide trajectory ..." print("Plan guide trajectory ...")
import anymal_plinth_path as tp import anymal_plinth_path as tp
print "Done." print("Done.")
import time import time
...@@ -43,12 +43,12 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) ...@@ -43,12 +43,12 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True) fullBody.usePosturalTaskContactCreation(True)
dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"} dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
print "Generate limb DB ..." print("Generate limb DB ...")
tStart = time.time() tStart = time.time()
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=50000) fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=50000)
tGenerate = time.time() - tStart tGenerate = time.time() - tStart
print "Done." print("Done.")
print "Databases generated in : "+str(tGenerate)+" s" print("Databases generated in : "+str(tGenerate)+" s")
fullBody.setReferenceConfig(q_ref) fullBody.setReferenceConfig(q_ref)
#define initial and final configurations : #define initial and final configurations :
...@@ -86,13 +86,13 @@ fullBody.setStartState(q_init,fullBody.limbs_names) ...@@ -86,13 +86,13 @@ fullBody.setStartState(q_init,fullBody.limbs_names)
fullBody.setEndState(q_goal,fullBody.limbs_names) fullBody.setEndState(q_goal,fullBody.limbs_names)
print "Generate contact plan ..." print("Generate contact plan ...")
tStart = time.time() tStart = time.time()
configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True) configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart tInterpolateConfigs = time.time() - tStart
print "Done. " print("Done. ")
print "Contact sequence computed in "+str(tInterpolateConfigs)+" s." print("Contact sequence computed in "+str(tInterpolateConfigs)+" s.")
print "number of configs :", len(configs) print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...") #raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs) #displayContactSequence(v,configs)
......
...@@ -87,7 +87,7 @@ ps.selectPathPlanner("DynamicPlanner") ...@@ -87,7 +87,7 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem : # Solve the planning problem :
t = ps.solve () t = ps.solve ()
print "Guide planning time : ",t print("Guide planning time : ",t)
#v.solveAndDisplay('rm',2,radiusSphere=0.01) #v.solveAndDisplay('rm',2,radiusSphere=0.01)
......
...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot ...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
from tools.display_tools import * from tools.display_tools import *
import time import time
print "Plan guide trajectory ..." print("Plan guide trajectory ...")