lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', compType = 'Equality')
lockpr2 = ps.lockPlanarJoint ('pr2/base_joint', 'pr2_lock', compType = 'Equality')
lockboth = lockpr2[:]; lockboth.extend (lockbox)
locklhand = ['l_l_finger','l_r_finger'];
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')
_["double_to_rightside"] = ""
_["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)
lockhands = ['l_l_finger', 'l_r_finger', 'r_l_finger', 'r_r_finger']
# 3}}}
# Create the graph. {{{3
cg.createNode(['both', 'right', 'left', 'free'])
cg.setConstraints(node='free', numConstraints=['box_placement'])
# This is not need as the object is locked in node free.
#graph.setNumericalConstraintsForPath (id["free"], ['box_placement'])
# Right hand {{{4
cg.setConstraints(node='right', grasp='r_grasp')
cg.createWaypointEdge('right', 'free', 'r_ungrasp', nb=1, weight=1)
cg.createWaypointEdge('free', 'right', 'r_grasp', nb=1, weight=10)
#graph.setLockedDofConstraints (id["r_ungrasp_w"], lockbox)
cg.setConstraints(edge='r_ungrasp_e1', lockDof=lockbox)
cg.setConstraints(node='r_ungrasp_n0',
pregrasp='r_pregrasp',
numConstraints=['box_placement'])
cg.setConstraints(edge='r_ungrasp_e0', lockDof=lockbox)
cg.setConstraints(edge='r_grasp_e1', lockDof=lockbox)
cg.setConstraints(node='r_grasp_n0', pregrasp='r_pregrasp')
cg.setConstraints(edge='r_grasp_e0', lockDof=lockbox)
# 4}}}
# Left hand {{{4
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_["move_free"]="move_free"
_["r_keep_grasp"]="r_keep_grasp"
_["l_keep_grasp"]="l_keep_grasp"
# 4}}}
cg.setTextToTeXTranslation (_)
cg.createNode (['both', 'right', 'left', 'free'])
cg.setConstraints (node='free', numConstraints=['box_placement'])
# Right hand {{{4
cg.setConstraints (node='right', grasps = ['r_grasp',])
cg.createWaypointEdge ('free', 'right', 'r_grasp', 1, 10, True)
cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_grasp_n0', pregrasps = ['r_pregrasp',])
cg.setConstraints (edge='r_grasp_e0', lockDof = lockbox)
# 4}}}
# Left hand {{{4
cg.setConstraints (node = 'left', grasps = ['l_grasp',])
cg.createWaypointEdge ('free', 'left', 'l_grasp', 1, 10, True, True)
cg.setConstraints (edge='l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_grasp_n0', pregrasps = ['l_pregrasp',])
cg.setConstraints (edge='l_grasp_e0', lockDof = lockbox)
# 4}}}
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_["move_free"]="move_free"
_["r_keep_grasp"]="r_keep_grasp"
_["l_keep_grasp"]="l_keep_grasp"
# 4}}}
cg.setTextToTeXTranslation (_)
cg.createNode (['both', 'right', 'left', 'free'])
cg.setConstraints (node='free', numConstraints=['box_placement'])
# Right hand {{{4
cg.setConstraints (node='right', grasps=['r_grasp'])
cg.createWaypointEdge ('free', 'right', 'r_grasp', nb=1, weight=10)
cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_grasp_n0', pregrasps = ['r_pregrasp'])
cg.setConstraints (edge='r_grasp_e0', lockDof = lockbox)
# 4}}}
# Left hand {{{4
cg.setConstraints (node = 'left', grasps = ['l_grasp'])
cg.createWaypointEdge ('free', 'left', 'l_grasp', 1, 10)
cg.setConstraints (edge='l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_grasp_n0', pregrasps = ['l_pregrasp'])
cg.setConstraints (edge='l_grasp_e0', lockDof = lockbox)
# 4}}}
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-romeo", q_init, 'romeo')
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode (['both', 'cup', 'fridge', 'free'])
# Cup and fridge {{{4
cg.setConstraints (node = 'both', grasps = [ 'l_cup_grasp', 'r_fridge_grasp', ])
cg.createWaypointEdge ('fridge', 'both', 'l_cup_grasp_both', 1, 10)
cg.setConstraints (edge='l_cup_grasp_both_e1', lockDof = lockCup)
cg.setConstraints (node='l_cup_grasp_both_n0', grasps = ['r_fridge_grasp'], pregrasps = ['l_cup_pregrasp',])
cg.setConstraints (edge='l_cup_grasp_both_e0', lockDof = lockCup)
cg.createWaypointEdge ('cup', 'both', 'r_fridge_grasp_both', 1, 10)
cg.setConstraints (edge='r_fridge_grasp_both_e1', lockDof = lockFridge)
cg.setConstraints (node='r_fridge_grasp_both_n0', grasps = ['l_cup_grasp'], pregrasps = ['r_fridge_pregrasp',])
cg.setConstraints (edge='r_fridge_grasp_both_e0', lockDof = lockFridge)
# 4}}}
# Cup {{{4
cg.setConstraints (node = 'cup', grasps = ['l_cup_grasp',])
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', grasps=[
'l_grasp',
])
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)
_["leftside_to_double"]=""
_["left_support"]=""
_["double_to_rightside"]=""
_["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"])
ps.createStaticStabilityConstraints("balance-romeo", q_init, 'romeo')
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode(['both', 'cup', 'fridge', 'free'])
# Cup and fridge {{{4
cg.setConstraints(node='both', grasps=[
'l_cup_grasp',
'r_fridge_grasp',
])
cg.createWaypointEdge('fridge', 'both', 'l_cup_grasp_both', 1, 10)
cg.setConstraints(edge='l_cup_grasp_both_e1', lockDof=lockCup)
cg.setConstraints(node='l_cup_grasp_both_n0',
grasps=['r_fridge_grasp'],
pregrasps=[
'l_cup_pregrasp',
])
cg.setConstraints(edge='l_cup_grasp_both_e0', lockDof=lockCup)
cg.createWaypointEdge('cup', 'both', 'r_fridge_grasp_both', 1, 10)
cg.setConstraints(edge='r_fridge_grasp_both_e1', lockDof=lockFridge)
cg.setConstraints(node='r_fridge_grasp_both_n0',
grasps=['l_cup_grasp'],
pregrasps=[
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
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')
ps.addPassiveDofs ('tom', jointNames ['tom'])
graph.createGrasp ('l2_grasp', 'tom/l_finger_tip', 'box/handle2',
passiveJoints = 'tom')
graph.createGrasp ('l1_grasp', 'tom/l_finger_tip', 'box/handle',
passiveJoints = 'tom')
graph.createGrasp ('r2_grasp', 'tom/r_finger_tip', 'box/handle2',
passiveJoints = 'tom')
graph.createGrasp ('r1_grasp', 'tom/r_finger_tip', 'box/handle',
passiveJoints = 'tom')
graph.createPreGrasp ('l2_pregrasp', 'tom/l_finger_tip', 'box/handle2')
graph.createPreGrasp ('l1_pregrasp', 'tom/l_finger_tip', 'box/handle')
graph.createPreGrasp ('r2_pregrasp', 'tom/r_finger_tip', 'box/handle2')
graph.createPreGrasp ('r1_pregrasp', 'tom/r_finger_tip', 'box/handle')
graph.createWaypointEdge ('free', 'hold_box_l1', 'grasp_box_l1',
nb=1, weight=10)
graph.createWaypointEdge ('free', 'hold_box_l2', 'grasp_box_l2',
nb=1, weight=10)
graph.createWaypointEdge ('free', 'hold_box_r1', 'grasp_box_r1',
nb=1, weight=10)
graph.createWaypointEdge ('free', 'hold_box_r2', 'grasp_box_r2',
nb=1, weight=10)
graph.createEdge ('free', 'free', 'move_free', 1)
graph.createEdge ('hold_box_l1', 'hold_box_l1', 'keep_grasp_l1', 5)
graph.createEdge ('hold_box_l2', 'hold_box_l2', 'keep_grasp_l2', 5)
graph.createEdge ('hold_box_r1', 'hold_box_r1', 'keep_grasp_r1', 5)
graph.createEdge ('hold_box_r2', 'hold_box_r2', 'keep_grasp_r2', 5)
graph.setConstraints (graph=True, lockDof = openLeftHand + openRightHand +
lockBase)
ps.createLockedJointConstraint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJointConstraint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJointConstraint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJointConstraint \
('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')
