#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) print("--- %s hours ---" % hours)