val = v ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints('balance-romeo', q, 'romeo', type=ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints(numConstraints=ps.balanceConstraints(), lockedJoints=lockHands) # build graph rules = [ Rule([ "romeo/l_hand", "romeo/r_hand", ], ["placard/low", ""], True), Rule([ "romeo/l_hand", "romeo/r_hand", ], ["", "placard/high"], True), Rule([ "romeo/l_hand", "romeo/r_hand",
# Loops {{{4 cg.createEdge ('free', 'free', 'move_free', 1) cg.createEdge ('cup', 'cup', 'cup_keep_grasp', 10) cg.createEdge ('fridge', 'fridge', 'fridge_keep_grasp', 10) cg.createEdge ('both', 'both', 'both_keep_grasp', 1) cg.setConstraints (edge='move_free', lockDof = lockFridge + lockCup) cg.setConstraints (edge='cup_keep_grasp', lockDof = lockFridge) cg.setConstraints (edge='fridge_keep_grasp', lockDof = lockCup) # 4}}} cg.setConstraints (graph = True, lockDof = lockAll, numConstraints=ps.balanceConstraints ()) # 3}}} #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] ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) cg.client.graph.setTargetNodeList (cg.subGraphId,
val = [v,] else: val = v; ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom ('romeo', ['romeo/root_joint']) q = robot.getInitialConfig () r = placard.rank q [r:r+3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints ('balance-romeo', q, 'romeo', type = ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints (numConstraints = ps.balanceConstraints (), lockedJoints = lockHands) # build graph rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True), Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True), Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True), ] grippers = ['romeo/r_hand', 'romeo/l_hand'] handlesPerObjects = [placard.handles.values ()] cg = ConstraintGraph.buildGenericGraph (robot, "graph", grippers, [placard.name,], handlesPerObjects,
graph.setNumericalConstraints(id["screwgun"], ['left-hand-grasp']) graph.setNumericalConstraintsForPath(id["screwgun"], ['left-hand-grasp-passive']) graph.setLockedDofConstraints(id["move_free"], lockscrewgun) graph.setLockedDofConstraints(id["pregrasp"], lockscrewgun) graph.setLockedDofConstraints(id["unpregrasp"], lockscrewgun) #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)
cg.createEdge('free', 'free', 'move_free', 1) cg.createEdge('cup', 'cup', 'cup_keep_grasp', 10) cg.createEdge('fridge', 'fridge', 'fridge_keep_grasp', 10) cg.createEdge('both', 'both', 'both_keep_grasp', 1) cg.setConstraints(edge='move_free', lockDof=lockFridge + lockCup) cg.setConstraints(edge='cup_keep_grasp', lockDof=lockFridge) cg.setConstraints(edge='fridge_keep_grasp', lockDof=lockCup) # 4}}} cg.setConstraints(graph=True, lockDof=lockAll, numConstraints=ps.balanceConstraints()) # 3}}} #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] ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) cg.client.graph.setTargetNodeList(cg.subGraphId, [
# Create the graph of constraints {{{3 graph.createNode (["screwgun", "free"]) graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1) graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5) graph.createEdge ('free', 'free', 'move_free', 5) graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10) graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5) graph.setConstraints (node='screwgun', grasp='l_grasp') graph.setConstraints (edge='move_free', lockDof = lockscrewgun) graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun) graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun) graph.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun) graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ()) # 3}}} ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) r = vf.createRealClient() pp = PathPlayer (robot.client.basic, r)
# Create the graph of constraints {{{3 graph.createNode (["screwgun", "free"]) graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1) graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5) graph.createEdge ('free', 'free', 'move_free', 5) graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10) graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5) graph.setConstraints (node='screwgun', grasp='l_grasp') graph.setConstraints (edge='move_free', lockDof = lockscrewgun) graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun) graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun) graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun) graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ()) # 3}}} ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.gepetto import PathPlayer pp = PathPlayer (robot.client.basic, r)
graph.setNumericalConstraints (id["prescrewgun"], ['left-hand-pregrasp', 'left-hand-pregrasp/ineq_0', 'left-hand-pregrasp/ineq_0.1']) #graph.setNumericalConstraints (id["prescrewgun"], ['left-hand-pregrasp', 'left-hand-pregrasp/ineq_0']) graph.setNumericalConstraintsForPath (id["prescrewgun"], ['left-hand-pregrasp-passive']) graph.setNumericalConstraints (id["screwgun"], ['left-hand-grasp']) graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive']) graph.setLockedDofConstraints (id["move_free"], lockscrewgun) graph.setLockedDofConstraints (id["pregrasp"], lockscrewgun) graph.setLockedDofConstraints (id["unpregrasp"], lockscrewgun) #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)