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),
)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setRandomSeed(54)
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
# Delete intermediate non optimized paths
for i in range(ps.numberPaths() - 1):
ps.erasePath(0)
# Generate N random configurations
N = args.N
if N != 0:
configs = [q[::]]
configs += shootRandomConfigs(ps, graph, configs[0], N)
configs = orderConfigurations(ps, configs)
configs.append(initConf)
else:
#read configurations in a file
configs = readConfigsInFile('./data/all-configurations.csv')
buildRoadmap(configs)
visitConfigurations(ps, configs)
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)