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"]])