Maze easy example

script/scenarios/memmo/talos_mazeEas_path.py

@@ -13,7 +13,7 @@ mu=0.5# coefficient of friction
 # Creating an instance of the helper class, and loading the robot
 rbprmBuilder = Robot()
 # Define bounds for the root : bounding box of the scenario
-root_bounds = [0,12,0.,2., 0.98, 0.98]
+root_bounds = [0,18.5,0.,24., 0.98, 0.98]
 rbprmBuilder.setJointBounds ("root_joint", root_bounds)
 # As this scenario only consider walking, we fix the DOF of the torso :
 rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
@@ -99,16 +99,24 @@ def generate_random_conf(bounds):
 	    print "Getting invalid config. try again."
 	    print message
 #set init
-q_init = generate_random_conf(root_bounds)
-v (q_init)
-ps.setInitialConfig (q_init)
-print "Press ENTER to continue"
-raw_input()
+while(True):
+    q_init = generate_random_conf(root_bounds)
+    v (q_init)
+    ps.setInitialConfig (q_init)
+    print "Accept?y/n"
+    user_in = raw_input()
+    if user_in == 'y':
+        break
+
+
 #set goal
-q_goal = generate_random_conf(root_bounds)
-v (q_goal)
-print "Press ENTER to continue"
-raw_input()
+while(True):
+    q_goal = generate_random_conf(root_bounds)
+    v (q_goal)
+    print "Accept?y/n"
+    user_in = raw_input()
+    if user_in == 'y':
+        break
 
 ps.resetGoalConfigs()
 ps.addGoalConfig (q_goal)
@@ -127,6 +135,14 @@ ps.selectPathPlanner("DynamicPlanner")
 # Solve the planning problem :
 #ps.setMaxIterPathPlanning(1000)
 t = ps.solve ()
+#tic = time.time()
+#v.solveAndDisplay('rm',5,0.01)
+#ps.optimizePath(0)
+#print "solve in " + str(time.time()-tic)
+
+#raw_input()
+
+
 print "Guide planning time : ",t
 
 

script/scenarios/memmo/talos_randomMove_flat.py

@@ -108,7 +108,10 @@ floor_Z = 0.
 s0 = sampleRandomStateFlatFloor(fullBody,limbsInContact,floor_Z)
 
 q_init = s0.q()
+<<<<<<< HEAD
 
+=======
+>>>>>>> 73ceb29917481d69e8c03ef7bba36d22e18d9958
 q_goal = q_ref[::]
 q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
 q_goal[2] = q_ref[2]

script/tools/surfaces_from_path.py

@@ -86,6 +86,7 @@ def getMergedPhases (seqs):
     nseqs.append(nseq)  
   return nseqs    
 
+
 def computeRootYawAngleBetwwenConfigs(q0,q1):
   quat0 = Quaternion(q0[6],q0[3],q0[4],q0[5])
   quat1 = Quaternion(q1[6],q1[3],q1[4],q1[5])