script/scenarios/sandbox/dynamic/parse_bench.py → script/dynamic/parse_bench.py

@@ -68,41 +68,41 @@ def parseBenchmark(tPlanningTab):
             dic[i][2] = dic[i][2]+it
 
 
-  print "Benchmark results (average, in ms): \n"
+  print("Benchmark results (average, in ms): \n")
 
 
   for i in range(0,len(dic)):
     if(dic[i][2] > 0):
       t = dic[i][1] / dic[i][2]
-      print dic[i][0],t
-      print str(dic[i][2])+" iterations"
+      print(dic[i][0],t)
+      print(str(dic[i][2])+" iterations")
 
   if tPlanning > 0:
-    print "\nDecomposition of the total planning time of "+str(tPlanning)+"s:"
+    print("\nDecomposition of the total planning time of "+str(tPlanning)+"s:")
     mt = tPlanning*1000.
-    print "# One step : "+str((dic[0][1]/mt)*100.)+" %"
-    print "\t * Random shooting : "+str((dic[1][1]/mt)*100.)+" %"
-    print "\t \t - collision checks : "+str((dic[14][1]/mt)*100.)+" %"
-    print "\t * Nearest search  : "+str((dic[2][1]/mt)*100.)+" %"
-    print "\t * extend          : "+str((dic[3][1]/mt)*100.)+" %"
-    print "\t \t - steering kino (core) : "+str((dic[4][1]/mt)*100.)+" %"
-    print "\t \t - fill node matrice    : "+str((dic[5][1]/mt)*100.)+" %"
-    print "\t \t \t . compute intersection (fcl) : "+str((dic[6][1]/mt)*100.)+" %"
-    print "\t \t - compute a max        : "+str((dic[8][1]/mt)*100.)+" %"
-    print "\t \t - check accelerations  : "+str((dic[9][1]/mt)*100.)+" %"
-    print "\t * path validation : "+str((dic[10][1]/mt)*100.)+" %"
-    print "\t \t - Dynamic validation : "+str((dic[11][1]/mt)*100.)+" %"
-    print "# Random shortcut       : "+str((dic[13][1]/mt)*100.)+" %"
-    print "# Orientation optimizer : "+str((dic[12][1]/mt)*100.)+" %"
-
-
-    print "% Is Reachable test     : called "+str(dic[15][2])+" times, average of "+str(dic[15][1]/dic[15][2])+" ms."
-    print "% QP solver isReachable : called "+str(dic[16][2])+" times, average of "+str(dic[16][1]/dic[16][2])+" ms." 
-    print "% stability constraints : called "+str(dic[17][2])+" times, average of "+str(dic[17][1]/dic[17][2])+" ms." 
-    print "% kinematic constraints : called "+str(dic[18][2])+" times, average of "+str(dic[18][1]/dic[18][2])+" ms." 
-    print "% stack matrices        : called "+str(dic[19][2])+" times, average of "+str(dic[19][1]/dic[19][2])+" ms." 
-    print "# Is Reachable Dynamic  : called "+str(dic[20][2])+" times, average of "+str(dic[20][1]/dic[20][2])+" ms." 
-    print "# Qp transition         : called "+str(dic[21][2])+" times, average of "+str(dic[21][1]/dic[21][2])+" ms." 
+    print("# One step : "+str((dic[0][1]/mt)*100.)+" %")
+    print("\t * Random shooting : "+str((dic[1][1]/mt)*100.)+" %")
+    print("\t \t - collision checks : "+str((dic[14][1]/mt)*100.)+" %")
+    print("\t * Nearest search  : "+str((dic[2][1]/mt)*100.)+" %")
+    print("\t * extend          : "+str((dic[3][1]/mt)*100.)+" %")
+    print("\t \t - steering kino (core) : "+str((dic[4][1]/mt)*100.)+" %")
+    print("\t \t - fill node matrice    : "+str((dic[5][1]/mt)*100.)+" %")
+    print("\t \t \t . compute intersection (fcl) : "+str((dic[6][1]/mt)*100.)+" %")
+    print("\t \t - compute a max        : "+str((dic[8][1]/mt)*100.)+" %")
+    print("\t \t - check accelerations  : "+str((dic[9][1]/mt)*100.)+" %")
+    print("\t * path validation : "+str((dic[10][1]/mt)*100.)+" %")
+    print("\t \t - Dynamic validation : "+str((dic[11][1]/mt)*100.)+" %")
+    print("# Random shortcut       : "+str((dic[13][1]/mt)*100.)+" %")
+    print("# Orientation optimizer : "+str((dic[12][1]/mt)*100.)+" %")
+
+
+    print("% Is Reachable test     : called "+str(dic[15][2])+" times, average of "+str(dic[15][1]/dic[15][2])+" ms.")
+    print("% QP solver isReachable : called "+str(dic[16][2])+" times, average of "+str(dic[16][1]/dic[16][2])+" ms.") 
+    print("% stability constraints : called "+str(dic[17][2])+" times, average of "+str(dic[17][1]/dic[17][2])+" ms.") 
+    print("% kinematic constraints : called "+str(dic[18][2])+" times, average of "+str(dic[18][1]/dic[18][2])+" ms.") 
+    print("% stack matrices        : called "+str(dic[19][2])+" times, average of "+str(dic[19][1]/dic[19][2])+" ms.") 
+    print("# Is Reachable Dynamic  : called "+str(dic[20][2])+" times, average of "+str(dic[20][1]/dic[20][2])+" ms.") 
+    print("# Qp transition         : called "+str(dic[21][2])+" times, average of "+str(dic[21][1]/dic[21][2])+" ms.") 
 
 
 

script/scenarios/sandbox/dynamic/plateform_hand_hrp2_interp.py → script/dynamic/plateform_hand_hrp2_interp.py

@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
 import time
 from constraint_to_dae import *
 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
-from display_tools import *
+from hpp.corbaserver.rbprm.tools.display_tools import *
 import plateform_hand_hrp2_path as tp
 import time
 
@@ -79,10 +79,10 @@ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 100000,
 
 
 tGenerate =  time.time() - tStart
-print "generate databases in : "+str(tGenerate)+" s"
+print("generate databases in : "+str(tGenerate)+" s")
 
 
-q_0 = fullBody.getCurrentConfig(); 
+q_0 = fullBody.getCurrentConfig();
 #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
 
 
@@ -150,7 +150,7 @@ import fullBodyPlayerHrp2
 tStart = time.time()
 configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = False)
 tInterpolateConfigs = time.time() - tStart
-print "number of configs :", len(configsFull)
+print("number of configs :", len(configsFull))
 
 """
 q_init[0] += 0.05
@@ -231,7 +231,7 @@ moveSphere('s',r,p)
 
 sE,success = StateHelper.addNewContact(sE,lLegId,p,n)
 assert(success)
-p = [1.15,-0.1,0] 
+p = [1.15,-0.1,0]
 sfe, success = StateHelper.addNewContact(sE,rLegId,p,n)
 assert(success)
 
@@ -247,14 +247,14 @@ assert(success)
 
 n = [0,0,1]
 p = [1.15,0.1,0]
-moveSphere('s',r,p) 
+moveSphere('s',r,p)
 
 sE2,success = StateHelper.addNewContact(sE,lLegId,p,n)
 
 
 n = [0,0,1]
 p = [1.15,-0.1,0]
-moveSphere('s',r,p) 
+moveSphere('s',r,p)
 
 
 sEf,success = StateHelper.addNewContact(sE2,lLegId,p,n)

script/scenarios/sandbox/dynamic/plateform_hrp2_interp_testTransition.py → script/dynamic/plateform_hrp2_interp_testTransition.py

@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
 import time
 from constraint_to_dae import *
 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
-from display_tools import *
+from hpp.corbaserver.rbprm.tools.display_tools import *
 import plateform_hrp2_path as tp
 import time
 
@@ -65,14 +65,14 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fi
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 fullBody.setReferenceConfig (q_ref)
-## Add arms (not used for contact) : 
+## Add arms (not used for contact) :
 
 
 tGenerate =  time.time() - tStart
-print "generate databases in : "+str(tGenerate)+" s"
+print("generate databases in : "+str(tGenerate)+" s")
 
 
-q_0 = fullBody.getCurrentConfig(); 
+q_0 = fullBody.getCurrentConfig();
 #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
 
 
@@ -217,7 +217,7 @@ moveSphere('s',r,p)
 
 sE,success = StateHelper.addNewContact(si,lLegId,p,n)
 assert(success)
-p = [1.15,-0.1,0] 
+p = [1.15,-0.1,0]
 sfe, success = StateHelper.addNewContact(sE,rLegId,p,n)
 assert(success)
 
@@ -253,7 +253,7 @@ cs = generateContactSequence(fullBody,configs,beginState, endState,r)
 
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
 cs.saveAsXML(filename, "ContactSequence")
-print "save contact sequence : ",filename
+print("save contact sequence : ",filename)
 
 
 

script/scenarios/sandbox/dynamic/plateform_hrp2_interp_testTransition_muscod.py → script/dynamic/plateform_hrp2_interp_testTransition_muscod.py

@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
 import time
 from constraint_to_dae import *
 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
-from display_tools import *
+from hpp.corbaserver.rbprm.tools.display_tools import *
 import plateform_hrp2_path as tp
 import time
 
@@ -65,14 +65,14 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fi
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 fullBody.setReferenceConfig (q_ref)
-## Add arms (not used for contact) : 
+## Add arms (not used for contact) :
 
 
 tGenerate =  time.time() - tStart
-print "generate databases in : "+str(tGenerate)+" s"
+print("generate databases in : "+str(tGenerate)+" s")
 
 
-q_0 = fullBody.getCurrentConfig(); 
+q_0 = fullBody.getCurrentConfig();
 #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
 
 
@@ -201,7 +201,7 @@ moveSphere('s',r,p)
 
 sE,success = StateHelper.addNewContact(sE,lLegId,p,n)
 assert(success)
-p = [1.15,-0.1,0] 
+p = [1.15,-0.1,0]
 sfe, success = StateHelper.addNewContact(sE,rLegId,p,n)
 assert(success)
 
@@ -251,7 +251,7 @@ cs = generateContactSequence(fullBody,configs,beginState, endState)
 
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
 cs.saveAsXML(filename, "ContactSequence")
-print "save contact sequence : ",filename
+print("save contact sequence : ",filename)
 
 
 

script/scenarios/sandbox/dynamic/prepareJump_hyq_interpKino.py → script/dynamic/prepareJump_hyq_interpKino.py

@@ -87,7 +87,7 @@ r(q_init)
 configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 3, filterStates = True)
 
 
-print "configs size = ",len(configs)
+print("configs size = ",len(configs))
 r(configs[-1])
 
 
@@ -95,7 +95,7 @@ r(configs[-1])
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (fullBody.client.basic, r)
 
-from fullBodyPlayer import Player
+from .fullBodyPlayer import Player
 player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=dynamic,pathId = 0)
 
 #player.displayContactPlan()

script/scenarios/sandbox/dynamic/sarpa_hrp2_interStatic.py → script/dynamic/sarpa_hrp2_interStatic.py

@@ -77,7 +77,7 @@ fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
 
 
 tGenerate =  time.time() - tStart
-print "generate databases in : "+str(tGenerate)+" s"
+print("generate databases in : "+str(tGenerate)+" s")
 
 
 """
@@ -147,8 +147,8 @@ import fullBodyPlayerHrp2
 tStart = time.time()
 configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = False)
 tInterpolate = time.time()-tStart
-print "number of configs : ", len(configs)
-print "generated in "+str(tInterpolate)+" s"
+print("number of configs : ", len(configs))
+print("generated in "+str(tInterpolate)+" s")
 r(configs[len(configs)-2])
 
 
@@ -167,7 +167,7 @@ from generate_contact_sequence import *
 cs = generateContactSequence(fullBody,configs[:5],r)
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
 cs.saveAsXML(filename, "ContactSequence")
-print "save contact sequence : ",filename
+print("save contact sequence : ",filename)
 
 
 """