for e in [
'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)))
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q0_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q2)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q2_proj = res [1]
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (20):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q0_proj)
ps.addGoalConfig (q2_proj)
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)/20.))
print ("Average number nodes: " + str (totalNumberNodes/20.))
# r = vf.createViewer()
# pp = PathPlayer (robot.client.basic, r)
we = dict ()
we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1)
we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
graph.createEdge ('free', 'free', 'move_free', 1)
graph.createEdge ('box', 'box', 'keep_grasp', 5)
graph.createLevelSetEdge ('box', 'box', 'keep_grasp_ls', 10)
# 3}}}
# Set the constraints of the component of the graph. {{{3
graph.setConstraints (node='box', grasp='l_grasp')
graph.setConstraints (edge='move_free', lockDof = lockbox)
graph.setConstraints (edge="ungrasp_e1", lockDof = lockbox)
graph.setConstraints (node="ungrasp_n0", pregrasp = 'l_pregrasp')
graph.setConstraints (edge="ungrasp_e0", lockDof = lockbox)
graph.setConstraints (edge="grasp_e1", lockDof = lockbox)
graph.setConstraints (node="grasp_n0", pregrasp = 'l_pregrasp')
graph.setConstraints (edge="grasp_e0", lockDof = lockbox)
graph.setLevelSetConstraints ("keep_grasp_ls", lockDof = lockbox)
graph.setConstraints (graph = True, lockDof = locklhand)
# 3}}}
# 2}}}
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# 1}}}
# vim: foldmethod=marker foldlevel=1
res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_goal)
if not res[0]:
raise Exception('Goal configuration could not be projected.')
q_goal_proj = res[1]
res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q2)
if not res[0]:
raise Exception('Goal configuration could not be projected.')
q2_proj = res[1]
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(20):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_goal_proj)
ps.addGoalConfig(q2_proj)
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) / 20.))
print("Average number nodes: " + str(totalNumberNodes / 20.))
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal_monotone)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_monotone_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal_inverted)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_inverted_proj = res [1]
ps.setInitialConfig (q_init_proj)
# ps.addGoalConfig (q_goal_monotone_proj)
ps.addGoalConfig (q_goal_inverted_proj)
# from hpp.corbaserver import Benchmark
import sys
from hpp.corbaserver import Benchmark
b = Benchmark (robot.client.basic, robot, ps)
b.seedRange = xrange (20)
b.iterPerCase = 1
b.tryResumeAndDelete ()
try:
b.do()
except:
id["move_free" ] = graph.createEdge (id["free"], id["free"], "move_free" , 1, False) id["keep_grasp"] = graph.createEdge (id["box" ], id["box" ], "keep_grasp", 1, False) if withLevelSetEgde: id["keep_grasp_ls"] = graph.createLevelSetEdge (id["box" ], id["box" ], "keep_grasp_ls", 1, False) if withWaypoint: id[ "grasp"] = graph.createWaypointEdge (id["free"], id["box"], "grasp", 10, True) id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint (id["grasp"]) else: id[ "grasp"] = graph.createEdge (id["free"], id["box"], "grasp", 10, True) graph.setNumericalConstraints (id["box"], ['grasp']) graph.setNumericalConstraintsForPath (id["box"], ['grasp-passive']) graph.setLockedDofConstraints (id["move_free"], lockbox) graph.setLockedDofConstraints (id["grasp"], lockbox) graph.setLockedDofConstraints (id["ungrasp"], lockbox) if withWaypoint: graph.setNumericalConstraints (id["grasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1']) graph.setLockedDofConstraints (id["grasp_w"], lockbox) if withLevelSetEgde: graph.setLevelSetConstraints (id["keep_grasp_ls"], [], lockbox) manip = robot.client.manipulation p.setInitialConfig (qinit) p.addGoalConfig (qgoal) if not withLevelSetEgde: manip.graph.statOnConstraint (id["grasp"])
foot_placement + arm_locked))
graph.initialize()
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setRandomSeed(54)
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
# Delete intermediate non optimized paths
for i in range(ps.numberPaths() - 1):
ps.erasePath(0)
# Generate N random configurations
N = args.N
if N != 0:
configs = [q[::]]
configs += shootRandomConfigs(ps, graph, configs[0], N)
configs = orderConfigurations(ps, configs)
configs.append(initConf)
else:
#read configurations in a file
# cg.graph.setLockedDofConstraints (cg.graphId , lockAll)
# 3}}}
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q0)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q0_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q1)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q1_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q2)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q2_proj = res [1]
ps.setInitialConfig (q_goal_proj)
ps.addGoalConfig (q2_proj)
'tiago/gripper grasps driller/handle : driller/drill_tip grasps skin/hole'
]:
graph.addConstraints(node=n,
constraints=Constraints(numConstraints=["gaze"]))
graph.initialize()
# Constraint in this state are explicit so ps.setMaxIterProjection(1) should not
# make it fail.
res, q1, err = graph.applyNodeConstraints(
'tiago/gripper grasps driller/handle', q0)
q1valid, msg = robot.isConfigValid(q1)
if not q1valid:
print(msg)
assert res
ps.setInitialConfig(q1)
if not isSimulation:
qrand = q1
for i in range(100):
q2valid, q2, err = graph.generateTargetConfig(
'driller/drill_tip > skin/hole | 0-0', q1, qrand)
if q2valid:
q2valid, msg = robot.isConfigValid(q2)
if q2valid:
break
qrand = robot.shootRandomConfig()
assert q2valid
if not isSimulation:
ps.addGoalConfig(q2)
if not res[0]:
raise Exception('Goal configuration could not be projected.')
q0_proj = res[1]
res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q2)
if not res[0]:
raise Exception('Goal configuration could not be projected.')
q2_proj = res[1]
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(20):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q0_proj)
ps.addGoalConfig(q2_proj)
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) / 20.))
print("Average number nodes: " + str(totalNumberNodes / 20.))
# r = vf.createViewer()
id["keep_grasp_ls"] = graph.createLevelSetEdge(id["box"], id["box"],
"keep_grasp_ls", 1, False)
if withWaypoint:
id["grasp"] = graph.createWaypointEdge(id["free"], id["box"], "grasp", 10,
True)
id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint(id["grasp"])
else:
id["grasp"] = graph.createEdge(id["free"], id["box"], "grasp", 10, True)
graph.setNumericalConstraints(id["box"], ['grasp'])
graph.setNumericalConstraintsForPath(id["box"], ['grasp-passive'])
graph.setLockedDofConstraints(id["move_free"], lockbox)
graph.setLockedDofConstraints(id["grasp"], lockbox)
graph.setLockedDofConstraints(id["ungrasp"], lockbox)
if withWaypoint:
graph.setNumericalConstraints(
id["grasp_w_node"],
['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints(id["grasp_w"], lockbox)
if withLevelSetEgde:
graph.setLevelSetConstraints(id["keep_grasp_ls"], [], lockbox)
manip = robot.client.manipulation
p.setInitialConfig(qinit)
p.addGoalConfig(qgoal)
if not withLevelSetEgde:
manip.graph.statOnConstraint(id["grasp"])
#graph.setNumericalConstraints (id["grasp"], cBalance)
#graph.setNumericalConstraints (id["ungrasp"], cBalance)
#graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
#graph.setLockedDofConstraints (id["ungrasp"], lockscrewgun)
graph.setLockedDofConstraints (id["grasp"], sum([lockscrewgun, lockbottompart],[]))
graph.setLockedDofConstraints (id["ungrasp"], sum([lockscrewgun, lockbottompart],[]))
graph.setLockedDofConstraints (id["keep_align"], lockscrewgun)
graph.setNumericalConstraints (id["graph"], p.balanceConstraints ())
graph.setLockedDofConstraints (id["graph"], locklhand)
manip = robot.client.manipulation
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qinit")
#qinit = res[1]
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qgoal")
#qgoal = res[1]
#p.setInitialConfig (qinit)
#p.addGoalConfig (qgoal)
p.setInitialConfig (q1)
p.addGoalConfig (q2)
# This projector tends to fail with probability 0.6 (with random configuration)
manip.graph.statOnConstraint ([id["grasp"]])
