'ur3/gripper < sphere0/handle | 0-0_ls',
'ur3/gripper < sphere1/handle | 0-1_ls'
]:
cg.setWeight(e, 100)
cg.initialize()
# Run benchmark
#
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = ps.numberNodes()
totalNumberNodes += n
print("Number nodes: " + str(n))
if args.N != 0:
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) /
float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envContactSurfaces)
factory.setObjects (objects, handlesPerObject, contactSurfacesPerObject)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints = Constraints \
(lockedJoints = locklhand))
cg.initialize ()
# 2}}}
res, q_init_proj, err = cg.applyNodeConstraints("free", q_init)
res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal)
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
print ps.solve()
ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"])
print ps.solve()
# 1}}}
v = vf.createViewer ()
v (q_init_proj)
pp = PathPlayer (v, robot.client.basic)
# vim: foldmethod=marker foldlevel=1
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20; success = 0
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
try:
t1 = dt.datetime.now ()
ps.solve ()
success += 1
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = ps.numberNodes ()
totalNumberNodes += n
print ("Number nodes: " + str(n))
except:
print ("Failed to plan path.")
print ("Number of successes: " + str (success))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (success)))
print ("Average number nodes: " + str (totalNumberNodes/float (success)))
])
graph.setConstraints(edge='move_free', lockDof=lockbox)
# graph.setConstraints (edge="ungrasp_e1", lockDof = lockbox)
# graph.setConstraints (node="ungrasp_n0", pregrasps = ['l_pregrasp',])
#graph.setConstraints (edge="ungrasp_e0", lockDof = lockboth)
# graph.setConstraints (edge="ungrasp_e0", lockDof = lockbox)
#graph.setConstraints (edge="grasp_e1", lockDof = lockboth)
graph.setConstraints(edge="grasp_e1", lockDof=lockbox)
graph.setConstraints(node="grasp_n0", pregrasps=[
'l_pregrasp',
])
graph.setConstraints(edge="grasp_e0", lockDof=lockbox)
#graph.client.graph.setLevelSetConstraints (graph.edges["keep_grasp_ls"], [], lockbox)
# graph.setLevelSetConstraints ("keep_grasp_ls", lockDof = lockbox)
graph.setConstraints(graph=True, lockDof=locklhand)
# 3}}}
# 2}}}
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
print ps.solve()
# 1}}}
#v = fk.createRealClient ()
#pp = PathPlayer (robot.client.basic, v)
# vim: foldmethod=marker foldlevel=1