Beispiel #1
0
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
]
r(q_goal)

ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "")

ps.selectPathOptimizer("None")
ps.selectPathValidation("Dichotomy", 0)

import datetime as dt

totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(20):
    ps.client.problem.clearRoadmap()
    ps.resetGoalConfigs()
    ps.setInitialConfig(q_init)
    ps.addGoalConfig(q_goal)
    t1 = dt.datetime.now()
    ps.solve()
    t2 = dt.datetime.now()
    totalTime += t2 - t1
q2 = [
    0.0, 0.0, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0,
    0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532,
    0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532,
    0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665,
    -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0
]

res = ps.applyConstraints(q2)
if res[0]:
    q2proj = res[1]
else:
    raise RuntimeError("Failed to apply constraint.")

ps.selectPathOptimizer("None")
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(20):
    ps.client.problem.clearRoadmap()
    ps.resetGoalConfigs()
    ps.setInitialConfig(q1proj)
    ps.addGoalConfig(q2proj)
    t1 = dt.datetime.now()
    ps.solve()
    t2 = dt.datetime.now()
    totalTime += t2 - t1
    print(t2 - t1)
    n = len(ps.client.problem.nodes())
    totalNumberNodes += n
print "Optim time: "+str(end-begin)
cl.problem.getIterationNumber ()
ps.pathLength(1)
#vPRM: 24 iter, 25.3s->21s, weighted Cost + comp linear error
#RRTc: 30 iter, 33.4s->14.9s, weighted Cost + comp linear error

begin=time.time()
ps.optimizePath (1)
end=time.time()
print "Optim time: "+str(end-begin)
cl.problem.getIterationNumber ()
ps.pathLength(2)

len(ps.getWaypoints (0))

ps.selectPathOptimizer('RandomShortcut')


r.startCapture ("capture","png")
pp(1)
r.stopCapture ()
#ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4


## DEBUG commands
cl.obstacle.getObstaclePosition('decor_base')
robot.getJointOuterObjects('j_object_one')
robot.getJointOuterObjects('j_object_two')
robot.getJointOuterObjects('j_object_three')
robot.isConfigValid(q1)
robot.distancesToCollision()