コード例 #1
0
ファイル: script.py プロジェクト: Laeksya/hpp_benchmark
        '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)))
コード例 #2
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
コード例 #3
0
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)))

コード例 #4
0
])
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