Example #1
0
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
Example #4
0
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:
Example #6
0
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"])
Example #7
0
                                             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)
Example #9
0
        '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)
Example #10
0
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()
Example #11
0
    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"])
Example #12
0
#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"]])