if not res[0]:
raise Exception('Goal configuration could not be projected.')
q_goal = res[1]
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))
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
if args.display:
v = vf.createViewer()
pp = PathPlayer(v, robot.client.basic)
if args.run:
pp(0)
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
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)))
# 1}}}
from hpp.gepetto import PathPlayer
v = vf.createViewer (); v (q_init)
pp = PathPlayer (robot.client.basic, v)
# vim: foldmethod=marker foldlevel=1
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (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))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
# 1}}}
from hpp.gepetto import PathPlayer
r = vf.createViewer (); r (q_init)
pp = PathPlayer (robot.client.basic, r)
# vim: foldmethod=marker foldlevel=1
graph.setConstraints (edge='grasp_box_l1_e0', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_l2_e0', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_r1_e0', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_r2_e0', lockDof = lockbox)
graph.setConstraints (node='hold_box_l1', grasps = ['l1_grasp',])
graph.setConstraints (node='hold_box_l2', grasps = ['l2_grasp',])
graph.setConstraints (node='hold_box_r1', grasps = ['r1_grasp',])
graph.setConstraints (node='hold_box_r2', grasps = ['r2_grasp',])
# graph.setConstraints (edge='keep_grasp_l1', lockDof = lockbox)
# graph.setConstraints (edge='keep_grasp_l2', lockDof = lockbox)
# graph.setConstraints (edge='keep_grasp_r1', lockDof = lockbox)
# graph.setConstraints (edge='keep_grasp_r2', lockDof = lockbox)
r = fk.createViewer ()
q_init = robot.halfSitting + [.75,0,.5,1,0,0,0]
res = ps.client.manipulation.problem.applyConstraints (graph.nodes ['free'], q_init)
q_init = res [1]
q_goal = robot.halfSitting + [.75,0,.3,1,0,0,0]
res = ps.client.manipulation.problem.applyConstraints (graph.nodes ['free'], q_goal)
q_goal = res [1]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
#ps.solve ()
#Create configurations and paths by hand.
res, q1, err = ps.client.manipulation.problem.applyConstraintsWithOffset \
(graph.edges ['grasp_box_r1_e0'], q_init, q_init)
res, i1, i2 = ps.client.manipulation.problem.buildAndProjectPath \
