res, pid, msg = ps.directPath(q1, q2, True) if res: print("Path from", i, "to", i + 1, ":", pid) paths.append(pid) else: print("Could not joint", i, "to", i + 1, ":", msg) failed = True ps.setParameter("SimpleTimeParameterization/safety", 0.5) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0) cleanPaths = True if not failed: # join path i0 = paths[0] for i in paths[1:]: ps.concatenatePath(i0, i) if cleanPaths: for k, i in enumerate(paths[1:]): ps.erasePath(i - k) ps.optimizePath(i0) print( "Optimized path:", ps.numberPaths() - 1, ",", ps.pathLength(ps.numberPaths() - 1), )
for j in notVisited: if dist[i, j] < m: m = dist[i, j] closest = j notVisited.remove(closest) visited.append(closest) visited.append(len(visited)) configs.append(initConf) print(visited) for i0, i1 in zip(visited, visited[1:]): q_init = configs[i0] q_goal = configs[i1] ps.resetGoalConfigs() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.solve() pid = ps.numberPaths() - 1 ps.concatenatePath(pathId, pid) ps.setParameter("SimpleTimeParameterization/safety", 0.5) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25) ps.addPathOptimizer("SimpleTimeParameterization") for i in range(ps.numberPaths() - 1, pathId, -1): ps.erasePath(i) ps.optimizePath(pathId)