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),
    )
Exemple #2
0
    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)