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)