position_emu= (0,-1,0.2,1,0,0,0)
cl.obstacle.moveObstacle ('emu_base', position_emu)
# Problem
ps.setInitialConfig (q3)
ps.addGoalConfig (q2) # q4 or q2
begin=time.time()
ps.solve ()
end=time.time()
print "Solving time: "+str(end-begin)
# Display spaceship environment and Emu guy
r.addObject('spaceShip','obstacle_base') # display
r.moveObject('spaceShip',(0,0,0,1,0,0,0))
r.addObject('emu','emu_base') # display
position_emu= (0,-1,0.2,1,0,0,0)
r.moveObject('emu',position_emu)
r(q3)
# Nodes from the roadmap
import time
nodes = cl.problem.nodes ()
len(nodes)
for n in nodes:
r (n)
time.sleep (.2)
## DEBUG commands
