factory.generate()
graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs))
free = "tiago/gripper grasps driller/handle"
loop_free = 'Loop | 0-0'
for n in graph.nodes.keys():
if n == free: continue
graph.addConstraints(node=n, constraints=Constraints(numConstraints=["look_at_gripper"]))
for e in graph.edges.keys():
graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["tiago_base"]))
#Add alignment constraints
for handle in part_handles:
addAlignmentConstrainttoEdge(ps, handle, graph)
graph.createNode('home', priority=1000)
graph.createEdge('home', 'home', 'move_base')
graph.createEdge('home', free , 'start_arm', isInNode="home")
graph.createEdge( free , 'home', 'end_arm', isInNode=free)
graph.addConstraints(node="home", constraints=Constraints(numConstraints=lock_arm+lock_head))
graph.addConstraints(edge="end_arm", constraints=Constraints(numConstraints=["tiago_base", "lock_part"]))
graph.addConstraints(edge="move_base", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))
graph.addConstraints(edge="start_arm", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))
cproblem = ps.hppcorba.problem.getProblem()
cgraph = cproblem.getConstraintGraph()
graph.initialize()
# 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')
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}}}
#('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True))
robot.client.basic.problem.createPositionConstraint \
('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True))
# 3}}}
# Create the graph. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
cg.createNode (['free'])
cg.setConstraints (node = 'free', numConstraints = ['pos'])
cg.setConstraints (graph = True, lockDof = lockjoints)
cg.createEdge ('free', 'free', 'move_free', 1)
# 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_proj)
ps.addGoalConfig (q_goal_proj)
cg.createWaypointEdge('both', 'left', 'b_r_ungrasp', 1, 1)
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)
cg.setContainingNode (ungraspEdge + "_e1", grasp)
cg.setShort (ungraspEdge + "_e1", True)
cg.setConstraints (edge=ungraspEdge + '_ls_e1', lockDof = otherLockBox)
cg.setContainingNode (ungraspEdge + "_ls_e1", grasp)
cg.setShort (ungraspEdge + "_ls_e1", True)
cg.setLevelSetFoliation (edge=ungraspEdge + '_ls_e1', condNC=[placement,],
paramLJ = lockBox)
cg.setConstraints (edge=ungraspEdge + '_e0', lockDof = otherLockBox)
cg.setContainingNode (ungraspEdge + "_e0", grasp)
genGrasp (1, lockBox1, lockBox2)
genGrasp (2, lockBox2, lockBox1)
cg.createEdge ('free', 'free', 'transit', 0, 'free')
cg.setConstraints (edge='transit', lockDof=lockBox1+lockBox2)
cg.createEdge ('grasp1', 'grasp1', 'transfer1', 0, 'grasp1')
cg.setConstraints (edge='transfer1', lockDof=lockBox2)
cg.createEdge ('grasp2', 'grasp2', 'transfer2', 0, 'grasp2')
cg.setConstraints (edge='transfer2', lockDof=lockBox1)
cg.setConstraints (graph = True, lockDof = lockHeadFingersAndLeftSide)
# 3}}}
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'], q0)
## Create constraint graph
graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[False, False, True, True, True, False,])
# Create complement constraint
ps.createTransformationConstraint ('placement/complement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[True, True, False, False, False, True,])
ps.setConstantRightHandSide ('placement', True)
pregrasps = ['l_pregrasp'])
cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1)
cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10)
cg.setConstraints (node='b_r_ungrasp_n0', grasps = ['l_grasp'],
pregrasps = ['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', grasps = ['l_grasp'],
pregrasps = ['r_pregrasp'])
# 4}}}
# Loops {{{4
cg.createEdge ('free', 'free', 'move_free', 1)
cg.createEdge ('left', 'left', 'l_keep_grasp', 5)
cg.createEdge ('right', 'right', 'r_keep_grasp', 5)
cg.setConstraints (edge='move_free', lockDof = lockbox)
# 4}}}
cg.setConstraints (graph = True, lockDof = lockAll)
# 3}}}
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
# Fridge {{{4
cg.setConstraints(node='fridge', grasps=[
'r_fridge_grasp',
])
cg.createWaypointEdge('free', 'fridge', 'r_fridge_grasp', 1, 10)
cg.setConstraints(edge='r_fridge_grasp_e1', lockDof=lockCup + lockFridge)
cg.setConstraints(node='r_fridge_grasp_n0', pregrasps=[
'r_fridge_pregrasp',
])
cg.setConstraints(edge='r_fridge_grasp_e0', lockDof=lockCup + lockFridge)
# 4}}}
# 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())
_["left_to_leftside"]=""
_["left_to_leftside_ls"]=""
_["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"])
_["left_to_leftside"]=""
_["left_to_leftside_ls"]=""
_["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","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)
('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')
graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun)
graph.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ())
# 3}}}
s = robot.getJointConfigSize(n)
r = robot.rankInConfiguration[n]
if n.startswith("talos/gripper_right"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
right_gripper_lock.append(n)
elif n.startswith("talos/gripper_left"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
left_gripper_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r + s])
cg = ConstraintGraph(robot, 'graph')
cg.createNode(['static stability'])
cg.createEdge(nodeFrom='static stability',
nodeTo='static stability',
name='move',
weight=1,
isInNode='static stability')
cg.setConstraints(node='static stability',
constraints=Constraints(
numConstraints=quasiStaticConstraints,
lockedJoints=left_gripper_lock + right_gripper_lock))
cg.initialize()
q_init = half_sitting[::]
q_goal = generateRandomConfig(robot, cg)
ps.setInitialConfig(q_init)
ps.resetGoalConfigs()
ps.addGoalConfig(q_goal)
ps.setParameter('SimpleTimeParameterization/order', 2)
ps.setParameter('SimpleTimeParameterization/maxAcceleration', .5)
ps.setParameter('SimpleTimeParameterization/safety', 0.5)
cg.setContainingNode(ungraspEdge + "_ls_e1", grasp)
cg.setShort(ungraspEdge + "_ls_e1", True)
cg.setLevelSetFoliation(edge=ungraspEdge + '_ls_e1',
condNC=[
placement,
],
paramLJ=lockBox)
cg.setConstraints(edge=ungraspEdge + '_e0', lockDof=otherLockBox)
cg.setContainingNode(ungraspEdge + "_e0", grasp)
genGrasp(1, lockBox1, lockBox2)
genGrasp(2, lockBox2, lockBox1)
cg.createEdge('free', 'free', 'transit', 0, 'free')
cg.setConstraints(edge='transit', lockDof=lockBox1 + lockBox2)
cg.createEdge('grasp1', 'grasp1', 'transfer1', 0, 'grasp1')
cg.setConstraints(edge='transfer1', lockDof=lockBox2)
cg.createEdge('grasp2', 'grasp2', 'transfer2', 0, 'grasp2')
cg.setConstraints(edge='transfer2', lockDof=lockBox1)
cg.setConstraints(graph=True, lockDof=lockHeadFingersAndLeftSide)
# 3}}}
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'], q0)
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)
graph.setConstraints (edge='move_free', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_l1_e1', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_l2_e1', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_r1_e1', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_r2_e1', lockDof = lockbox)
elif args.arm == 'right':
arm_locked = left_arm_lock
else:
arm_locked = list()
left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf)
com_constraint, foot_placement, foot_placement_complement = \
createQuasiStaticEquilibriumConstraint (ps, initConf)
gaze_constraint = createGazeConstraint(ps, args.arm)
waist_constraint = createWaistYawConstraint(ps)
graph = ConstraintGraph(robot, "graph")
graph.createNode(["free", "starting_state"])
graph.createEdge("starting_state",
"free",
"starting_motion",
isInNode="starting_state")
graph.createEdge("free",
"starting_state",
"go_to_starting_state",
isInNode="starting_state")
graph.createEdge("free", "free", "Loop | f", isInNode="starting_state")
# Set constraints
graph.addConstraints(
node="starting_state",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
_["left_to_leftside"] = ""
_["left_to_leftside_ls"] = ""
_["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", "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)
cg.setConstraints (edge='b_l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_l_grasp_n0', pregrasps = ['l_pregrasp',])
cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1, False, True)
cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10, True, True)
cg.setConstraints (node='b_r_ungrasp_n0', pregrasps = ['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', pregrasps = ['r_pregrasp',])
# 4}}}
# Loops {{{4
cg.createEdge ('free', 'free', 'move_free', 1, True)
cg.createEdge ('left', 'left', 'l_keep_grasp', 5, True)
cg.createEdge ('right', 'right', 'r_keep_grasp', 5, True)
cg.createEdge ('both', 'both', 'move_both', 1, True)
cg.setConstraints (edge='move_free', lockDof = lockbox)
# 4}}}
cg.setConstraints (graph = True, lockDof = lockAll)
# 3}}}
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
cg.setConstraints (node='l_cup_grasp_n0', pregrasps = ['l_cup_pregrasp',])
cg.setConstraints (edge='l_cup_grasp_e0', lockDof = lockCup + lockFridge)
# 4}}}
# Fridge {{{4
cg.setConstraints (node = 'fridge', grasps = ['r_fridge_grasp',])
cg.createWaypointEdge ('free', 'fridge', 'r_fridge_grasp', 1, 10)
cg.setConstraints (edge='r_fridge_grasp_e1', lockDof = lockCup + lockFridge)
cg.setConstraints (node='r_fridge_grasp_n0', pregrasps = ['r_fridge_pregrasp',])
cg.setConstraints (edge='r_fridge_grasp_e0', lockDof = lockCup + lockFridge)
# 4}}}
# 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}}}
## Create graph
graph = ConstraintGraph (robot, 'graph')
# Contraint of ball relative position while it is located in the gripper
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
# We are creating our graph as described in the instructions
graph.createNode (['grasp-placement', 'ball-above-ground','gripper-above-ball','grasp','placement'])
graph.createEdge ('grasp-placement','ball-above-ground','take-ball-up',1,'grasp')
graph.createEdge ('ball-above-ground','grasp-placement','put-ball-down',1,'grasp')
graph.createEdge ('grasp-placement','gripper-above-ball','move-gripper-up',1,'placement')
graph.createEdge ('gripper-above-ball','grasp-placement','grasp-ball',1,'placement')
graph.createEdge ('gripper-above-ball','placement','move-gripper-away',1,'placement')
graph.createEdge ('placement','gripper-above-ball','approach-ball',1,'placement')
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('ball-above-ground','grasp','take-ball-away',1,'grasp')
graph.createEdge ('grasp','ball-above-ground','approach-ground',1,'grasp')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
# placement constraint - ball is in horizontal plane with free rotation around z allowed
ps.createTransformationConstraint ('placement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[False, False, True, True, True, False,])
"grasp",
gripperName,
ballName,
ballInGripper,
6
* [
True,
],
)
# Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
graph.createNode(["grasp", "placement"])
graph.createEdge("placement", "placement", "transit", 1, "placement")
graph.createEdge("grasp", "grasp", "transfer", 1, "grasp")
graph.createEdge("placement", "grasp", "grasp-ball", 1, "placement")
graph.createEdge("grasp", "placement", "release-ball", 1, "grasp")
# Create transformation constraint : ball is in horizontal plane with free
# rotation around z
ps.createTransformationConstraint(
"placement",
"",
ballName,
[0, 0, 0.025, 0, 0, 0, 1],
[
False,
False,
True,
_["left_to_leftside"] = ""
_["left_to_leftside_ls"] = ""
_["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',
#('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True))
robot.client.basic.problem.createPositionConstraint \
('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True))
# 3}}}
# Create the graph. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
cg.createNode(['free'])
cg.setConstraints(node='free', numConstraints=['pos'])
cg.setConstraints(graph=True, lockDof=lockjoints)
cg.createEdge('free', 'free', 'move_free', 1)
# 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_proj)
ps.addGoalConfig(q_goal_proj)
cg.graph.setContainingNode (cg.edges[edgeUBN + "_e1"], cg.nodes[rN])
cg.graph.setShort (cg.edges[edgeUBN + "_e1"], True)
cg.setConstraints (edge=edgeUBN + '_ls_e1', lockDof = lFixedBox)
cg.graph.setContainingNode (cg.edges[edgeUBN + "_ls_e1"], cg.nodes[rN])
cg.graph.setShort (cg.edges[edgeUBN + "_ls_e1"], True)
cg.setLevelSetFoliation (edge=edgeUBN + '_ls_e1', condNC=[pcN,], paramLJ = lMovingBox) # , paramNC=[pcN + '/complement'])
cg.setConstraints (edge=edgeUBN + '_e0', lockDof = lFixedBox)
cg.graph.setContainingNode (cg.edges[edgeUBN + "_e0"], cg.nodes[rN])
# cg.graph.setShort (cg.edges["r_ungrasp_e0"], True)
genGrasp (1, lockBox1, lockBox2)
genGrasp (2, lockBox2, lockBox1)
cg.createEdge ('free', 'free', 'move_free', weight = 0)
cg.setConstraints (edge='move_free', lockDof=lockBox1+lockBox2)
cg.createEdge ('r1', 'r1', 'kg1', weight = 0)
cg.setConstraints (edge='kg1', lockDof=lockBox2)
cg.createEdge ('r2', 'r2', 'kg2', weight = 0)
cg.setConstraints (edge='kg2', lockDof=lockBox1)
cg.setConstraints (graph = True, lockDof = lockAll)
# cg.setConstraints (graph = True, numConstraints = ["gaze",], passiveJoints = ["box",], lockDof = lockAll)
# cg.graph.setNumericalConstraints (cg.graphId , ["gaze",], ["box",])
# cg.graph.setLockedDofConstraints (cg.graphId , lockAll)
# 3}}}
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
])
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)
#graph.setConstraints (edge="grasp_e1", lockDof = lockboth)
graph.setConstraints(edge="grasp_e1", lockDof=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')
graph.setConstraints (edge="grasp_e0", lockDof = lockbox)
graph.setLevelSetConstraints ("keep_grasp_ls", lockDof = lockbox)
graph.setConstraints (graph = True, lockDof = locklhand)
vf = ViewerFactory(ps)
left_arm_lock = createLeftArmLockedJoints(ps)
right_arm_lock = createRightArmLockedJoints(ps)
arm_locked = right_arm_lock if args.arm == 'left' else left_arm_lock
left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf)
com_constraint, foot_placement, foot_placement_complement = \
createQuasiStaticEquilibriumConstraint (ps, initConf)
gaze_constraint = createGazeConstraint(ps, args.arm)
waist_constraint = createWaistYawConstraint(ps)
graph = ConstraintGraph(robot, "graph")
graph.createNode(["free", "starting_state"])
graph.createEdge("starting_state",
"free",
"apply_constraints",
isInNode="starting_state")
graph.createEdge("free",
"starting_state",
"release_constraints",
isInNode="starting_state")
graph.createEdge("free", "free", "Loop | f", isInNode="starting_state")
# Set constraints
graph.addConstraints(
node="starting_state",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +