#qG = q2
#solver.loadObstacleFromUrdf("hpp-motion-prior","floor","")
solver.loadObstacleFromUrdf("hpp-motion-prior","floor_obstacle","")
#solver.loadObstacleFromUrdf("hpp-motion-prior","wall","")
## Add constraints
#qI[1]=1.0
solver.setInitialConfig (qI)
solver.addGoalConfig (qG)
#publisher(q)
#print q
#sys.exit(0)
qqinit = solver.getInitialConfig()[0:3]
qqgoal = solver.getGoalConfigs()[0][0:3]
print "planning from",qqinit,"to",qqgoal
#print "Irreducible Planner"
#solver.selectPathPlanner("PRM")
#solver.selectPathPlanner("DiffusingPlanner")
print "solve"
start_time = float(time.time())
solver.selectPathPlanner("IrreduciblePlanner")
solver.solve ()
end_time = float(time.time())
seconds = end_time - start_time
hours = seconds/3600
print("--- %s seconds ---" % seconds)
