Commit d8bc3c9d authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

Maze easy example

parent 80fa6d8f
......@@ -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
......
......@@ -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]
......
......@@ -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])
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment