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