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()
