ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
# 2}}}
# Create the constraint graph. {{{2
# Create nodes and edges. {{{3
graph.createNode (['box', 'free'])
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}}}
# 4}}}
# graph.setTextToTeXTranslation (_)
graph.createNode(["both", "left", "right"])
graph.createEdge('both', 'both', 'd2d', weight=0)
graph.createWaypointEdge('both',
'left',
'd2l',
nb=1,
isInNodeFrom=True,
weight=1)
graph.graph.isInNodeFrom(graph.edges["d2l_e1"], False)
graph.createEdge('left', 'both', 'l2d', isInNodeFrom=True, weight=1)
graph.createLevelSetEdge('left', 'both', 'l2d_ls', isInNodeFrom=True, weight=5)
graph.createEdge('left', 'left', 'l2l', weight=0)
graph.createWaypointEdge('both',
'right',
'd2r',
nb=1,
isInNodeFrom=True,
weight=1)
graph.graph.isInNodeFrom(graph.edges["d2r_e1"], False)
graph.createEdge('right', 'both', 'r2d', isInNodeFrom=True, weight=1)
graph.createLevelSetEdge('right',
'both',
'r2d_ls',
isInNodeFrom=True,
weight=5)
cg.setConstraints(edge='b_r_ungrasp_e1', lockDof=lockbox)
cg.setConstraints(node='b_r_ungrasp_n0',
grasp='l_grasp',
pregrasp='r_pregrasp')
cg.setConstraints(edge='b_r_ungrasp_e0', lockDof=lockbox)
cg.setConstraints(edge='b_r_grasp_e1', lockDof=lockbox)
cg.setConstraints(node='b_r_grasp_n0', grasp='l_grasp', pregrasp='r_pregrasp')
cg.setConstraints(edge='b_r_grasp_e0', lockDof=lockbox)
# 4}}}
# Loops {{{4
cg.createEdge('free', 'free', 'move_free', 1)
cg.createEdge('left', 'left', 'l_keep_grasp', 5)
cg.createLevelSetEdge('left', 'left', 'l_keep_grasp_ls', 10)
cg.createEdge('right', 'right', 'r_keep_grasp', 5)
cg.createLevelSetEdge('right', 'right', 'r_keep_grasp_ls', 10)
cg.setConstraints(edge='move_free', lockDof=lockbox)
cg.client.graph.setLevelSetConstraints(cg.edges['l_keep_grasp_ls'], [],
lockbox)
cg.client.graph.setLevelSetConstraints(cg.edges['r_keep_grasp_ls'], [],
lockbox)
# 4}}}
cg.setConstraints(graph=True, lockDof=lockhands)
# 3}}}
res = p.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_init)
cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10)
cg.setConstraints (edge='b_r_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_ungrasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='b_r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_grasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_grasp_e0', lockDof = lockbox)
# 4}}}
# Loops {{{4
cg.createEdge ('free', 'free', 'move_free', 1)
cg.createEdge ('left', 'left', 'l_keep_grasp', 5)
cg.createLevelSetEdge ('left', 'left', 'l_keep_grasp_ls', 10)
cg.createEdge ('right', 'right', 'r_keep_grasp', 5)
cg.createLevelSetEdge ('right', 'right', 'r_keep_grasp_ls', 10)
cg.setConstraints (edge='move_free', lockDof = lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['l_keep_grasp_ls'], [], lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox)
# 4}}}
cg.setConstraints (graph = True, lockDof = lockhands)
# 3}}}
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
# graph.setTextToTeXTranslation (_)
graph.createNode(["both_left", "both_right", "both", "left", "right"])
graph.createEdge('both', 'both', 'double', weight=0)
graph.createEdge('both', 'both_left', 'double_to_leftside', isInNodeFrom=True)
graph.createEdge('both_left', 'left', 'leftside_to_left', isInNodeFrom=False)
graph.createEdge('both_left', 'both', 'leftside_to_double', isInNodeFrom=False)
graph.createEdge('left',
'both_left',
'left_to_leftside',
isInNodeFrom=True,
weight=0)
graph.createLevelSetEdge('left',
'both_left',
'left_to_leftside_ls',
isInNodeFrom=True)
graph.createEdge('left', 'left', 'left_support', weight=0)
graph.createEdge('both',
'both_right',
'double_to_rightside',
isInNodeFrom=True)
graph.createEdge('both_right',
'right',
'rightside_to_right',
isInNodeFrom=False)
graph.createEdge('both_right',
'both',
'rightside_to_double',
isInNodeFrom=False)
_["rightside_to_right"]=""
_["right_to_rightside"]=""
_["right_to_rightside_ls"]=""
_["rightside_to_double"]=""
_["right_support"]=""
# 4}}}
# graph.setTextToTeXTranslation (_)
graph.createNode (["both","left","right"])
graph.createEdge ('both', 'both', 'd2d', weight = 0)
graph.createWaypointEdge ('both', 'left', 'd2l', nb = 1, isInNodeFrom = True, weight = 1)
graph.graph.isInNodeFrom (graph.edges["d2l_e1"], False)
graph.createEdge ('left', 'both', 'l2d', isInNodeFrom = True, weight = 1)
graph.createLevelSetEdge ('left', 'both', 'l2d_ls', isInNodeFrom = True, weight = 5)
graph.createEdge ('left', 'left', 'l2l', weight = 0)
graph.createWaypointEdge ('both', 'right', 'd2r', nb = 1, isInNodeFrom = True, weight = 1)
graph.graph.isInNodeFrom (graph.edges["d2r_e1"], False)
graph.createEdge ('right', 'both', 'r2d', isInNodeFrom = True, weight = 1)
graph.createLevelSetEdge ('right', 'both', 'r2d_ls', isInNodeFrom = True, weight = 5)
graph.createEdge ('right', 'right', 'r2r', weight = 0)
graph.setConstraints (node='both', numConstraints = bothfeet[0])
graph.setConstraints (node='d2l_n0', numConstraints = [ "leftfoot/com", "leftfoot/z", "leftfoot/rxry", "rightfoot/z", "rightfoot/rxry"])
graph.setConstraints (node='left', numConstraints = leftfoot[0])
graph.setConstraints (node='d2r_n0', numConstraints = ["rightfoot/com", "rightfoot/z", "rightfoot/rxry", "leftfoot/z", "leftfoot/rxry"])
graph.setConstraints (node='right', numConstraints = rightfoot[0])
graph.setConstraints (edge='d2d', numConstraints = bothfeet[1])
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
# 2}}}
# Create the constraint graph. {{{2
# Create nodes and edges. {{{3
graph.createNode (['box', 'free'])
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 = 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)
ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-hrp2", q_init, 'hrp2')
# 3}}}
# 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)
_["right_to_rightside"]=""
_["right_to_rightside_ls"]=""
_["rightside_to_double"]=""
_["right_support"]=""
# 4}}}
# graph.setTextToTeXTranslation (_)
graph.createNode (["both_left","both_right","both","left","right"])
graph.createEdge ('both', 'both', 'double', weight = 0)
graph.createEdge ('both', 'both_left', 'double_to_leftside', isInNodeFrom = True)
graph.createEdge ('both_left', 'left', 'leftside_to_left', isInNodeFrom = False)
graph.createEdge ('both_left', 'both', 'leftside_to_double', isInNodeFrom = False)
graph.createEdge ('left', 'both_left', 'left_to_leftside', isInNodeFrom = True, weight = 0)
graph.createLevelSetEdge ('left', 'both_left', 'left_to_leftside_ls', isInNodeFrom = True)
graph.createEdge ('left', 'left', 'left_support', weight = 0)
graph.createEdge ('both', 'both_right', 'double_to_rightside', isInNodeFrom = True)
graph.createEdge ('both_right', 'right', 'rightside_to_right', isInNodeFrom = False)
graph.createEdge ('both_right', 'both', 'rightside_to_double', isInNodeFrom = False)
graph.createEdge ('right', 'both_right', 'right_to_rightside', isInNodeFrom = True, weight = 0)
graph.createLevelSetEdge ('right', 'both_right', 'right_to_rightside_ls', isInNodeFrom = True)
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])
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.createStaticStabilityConstraints ("balance", q_init)
# 3}}}
# 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)
