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
graph.setConstraints(node='left', numConstraints=leftfoot[0])
graph.setConstraints(node='both_right',
numConstraints=[
"rightfoot/com", "rightfoot/z", "rightfoot/rxry",
"leftfoot/z", "leftfoot/rxry"
])
graph.setConstraints(node='right', numConstraints=rightfoot[0])
graph.setConstraints(edge='double', numConstraints=bothfeet[1])
graph.setConstraints(edge='double_to_leftside', numConstraints=bothfeet[1])
graph.setConstraints(edge='leftside_to_left', numConstraints=leftfoot[1])
graph.setConstraints(edge='leftside_to_double', numConstraints=bothfeet[1])
graph.setConstraints(edge='left_to_leftside', numConstraints=leftfoot[1])
graph.setConstraints(edge='left_to_leftside_ls', numConstraints=leftfoot[1])
graph.setLevelSetConstraints(edge='left_to_leftside_ls',
numConstraints=["rightfoot/xy"])
graph.setConstraints(edge='left_support', numConstraints=leftfoot[1])
graph.setConstraints(edge='double_to_rightside', numConstraints=bothfeet[1])
graph.setConstraints(edge='rightside_to_right', numConstraints=rightfoot[1])
graph.setConstraints(edge='rightside_to_double', numConstraints=bothfeet[1])
graph.setConstraints(edge='right_to_rightside', numConstraints=rightfoot[1])
graph.setConstraints(edge='right_to_rightside_ls', numConstraints=rightfoot[1])
graph.setLevelSetConstraints(edge='right_to_rightside_ls',
numConstraints=["leftfoot/xy"])
graph.setConstraints(edge='right_support', numConstraints=rightfoot[1])
lockDofs = lockhand[:]
lockDofs.append("all/ori_waist")
graph.setConstraints(graph=True, lockDof=lockDofs)
# 3}}}
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 = 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", pregrasp = '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)
# 1}}}
# vim: foldmethod=marker foldlevel=1
# 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)
graph.createEdge ('right', 'right', 'right_support', weight = 0)
graph.setConstraints (node='both', numConstraints = bothfeet[0])
graph.setConstraints (node='both_left', numConstraints = ["leftfoot/com", "leftfoot/z" , "leftfoot/rxry", "rightfoot/z" , "rightfoot/rxry"])
graph.setConstraints (node='left', numConstraints = leftfoot[0])
graph.setConstraints (node='both_right', numConstraints = ["rightfoot/com", "rightfoot/z" , "rightfoot/rxry", "leftfoot/z" , "leftfoot/rxry"])
graph.setConstraints (node='right', numConstraints = rightfoot[0])
graph.setConstraints (edge='double', numConstraints = bothfeet[1])
graph.setConstraints (edge='double_to_leftside', numConstraints = bothfeet[1])
graph.setConstraints (edge='leftside_to_left', numConstraints = leftfoot[1])
graph.setConstraints (edge='leftside_to_double', numConstraints = bothfeet[1])
graph.setConstraints (edge='left_to_leftside', numConstraints = leftfoot[1])
graph.setConstraints (edge='left_to_leftside_ls', numConstraints = leftfoot[1])
graph.setLevelSetConstraints (edge='left_to_leftside_ls', numConstraints = ["rightfoot/xy"])
graph.setConstraints (edge='left_support', numConstraints = leftfoot[1])
graph.setConstraints (edge='double_to_rightside', numConstraints = bothfeet[1])
graph.setConstraints (edge='rightside_to_right', numConstraints = rightfoot[1])
graph.setConstraints (edge='rightside_to_double', numConstraints = bothfeet[1])
graph.setConstraints (edge='right_to_rightside', numConstraints = rightfoot[1])
graph.setConstraints (edge='right_to_rightside_ls', numConstraints = rightfoot[1])
graph.setLevelSetConstraints (edge='right_to_rightside_ls', numConstraints = ["leftfoot/xy"])
graph.setConstraints (edge='right_support', numConstraints = rightfoot[1])
lockDofs = lockhand[:]
lockDofs.append ("all/ori_waist")
graph.setConstraints (graph=True, lockDof = lockDofs)
# 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)
