lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock')
lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock')
# 4}}}
# Create gaze constraints {{{4
ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode (['grasp2', 'grasp1', 'free'])
cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table'])
cg.setConstraints (node='grasp1', grasps = ['grasp1',])
cg.setConstraints (node='grasp2', grasps = ['grasp2',])
def genGrasp (iBox, lockBox, otherLockBox):
iStr = str(iBox)
grasp = 'grasp' + iStr
edgeBaseName = grasp+'_edge'
placement = 'box' + iStr + "_on_table"
box_above_table = 'box' + iStr + "_above_table"
# Free to right
cg.createWaypointEdge ('free', grasp, edgeBaseName, nb=3, weight=1,
isInNode = 'free', automaticBuilder=True)
cg.setConstraints (edge = edgeBaseName + '_e0',
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)
graph.setConstraints (node='grasp_box_l1_n0', pregrasps = ['l1_pregrasp',])
graph.setConstraints (node='grasp_box_l2_n0', pregrasps = ['l2_pregrasp',])
graph.setConstraints (node='grasp_box_r1_n0', pregrasps = ['r1_pregrasp',])
graph.setConstraints (node='grasp_box_r2_n0', pregrasps = ['r2_pregrasp',])
graph.setConstraints (edge='grasp_box_l1_e0', lockDof = lockbox)
graph.setConstraints (edge='grasp_box_l2_e0', lockDof = lockbox)
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["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)
lockrhand = ['r_l_finger', 'r_r_finger']
p.createLockedDofConstraint('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5,
0, 0)
p.createLockedDofConstraint('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5,
0, 0)
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'])
for i in xrange(K):
handlesPerObject.append ([boxes[i] + "/handle2"])
handles.append (boxes[i] + "/handle2")
shapes .append ([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule ([".*"], [".*"], True)]
ps.client.manipulation.graph.autoBuild ("graph",
grippers, boxes, handlesPerObject, shapes,
['table/pancake_table_table_top'],
rules)
# Get the built graph
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
cg.setConstraints (graph = True, lockDof = lockAll)
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)
import datetime as dt
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild('graph', grippers, boxes,
handlesPerObject, contactPerObject,
envSurfaces, rules)
cg = ConstraintGraph(robot, 'graph', makeGraph=False)
cg.setConstraints(graph=True, lockDof=locklhand)
cg.setWeight('pr2/l_gripper < box/handle | 0-0_ls', 0)
res, q_init, err = cg.applyNodeConstraints('free', q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints('free', q_goal)
if not res:
raise RuntimeError("Failed to project initial configuration")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setMaxIterPathPlanning(5000)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
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)
r = vf.createRealClient()
# 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)
graph.setConstraints(node="grasp_n0", pregrasps=[
'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)
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])
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])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild ('graph',
grippers, boxes, handlesPerObject, contactPerObject, envSurfaces, rules)
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
cg.setConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('pr2/l_gripper < box/handle | 0-0_ls', 0)
cg.setWeight ('Loop | f', 1)
res, q_init, err = cg.applyNodeConstraints ('free', q_init)
if not res:
raise RuntimeError ("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints ('free', q_goal)
if not res:
raise RuntimeError ("Failed to project initial configuration")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
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)
ps.solve()
pid = ps.numberPaths() - 1
if pid >= 0:
writeTrajInYaml(ps, pid, '/tmp/traj.yaml', .1)
# Create gaze constraints {{{4
ps.createPositionConstraint("gaze1", "baxter/display_joint",
"box1/base_joint_xyz", [0, 0, 0], [0, 0, 0],
[1, 0, 0])
ps.createPositionConstraint("gaze2", "baxter/display_joint",
"box2/base_joint_xyz", [0, 0, 0], [0, 0, 0],
[1, 0, 0])
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode(['grasp2', 'grasp1', 'free'])
cg.setConstraints(node='free',
numConstraints=['box1_on_table', 'box2_on_table'])
cg.setConstraints(node='grasp1', grasps=[
'grasp1',
])
cg.setConstraints(node='grasp2', grasps=[
'grasp2',
])
def genGrasp(iBox, lockBox, otherLockBox):
iStr = str(iBox)
grasp = 'grasp' + iStr
edgeBaseName = grasp + '_edge'
placement = 'box' + iStr + "_on_table"
box_above_table = 'box' + iStr + "_above_table"
])
ps.createTransformationConstraint('ball-above-ground', '', ballName,
[0, 0, 0.3, 0, 0, 0, 1],
[False, False, True, True, True, False])
ps.createTransformationConstraint('change-in-altitude-only', '', gripperName,
[0, 0, 0.3, 0, 0, 0, 1],
[True, True, False, True, True, True])
ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('change-in-altitude-only', False)
## Set constraints of nodes and edges
graph.setConstraints(node='placement', numConstraints=['placement'])
graph.setConstraints(node='grasp', numConstraints=['grasp'])
graph.setConstraints(node='ball-above-ground',
numConstraints=['grasp', 'ball-above-ground'])
graph.setConstraints(node='grasp-placement',
numConstraints=['grasp', 'placement'])
graph.setConstraints(node='gripper-above-ball',
numConstraints=['gripper-above-ball', 'placement'])
# These edges are in node 'placement'
graph.setConstraints(edge='transit', numConstraints=['placement/complement'])
graph.setConstraints(edge='move-gripper-away',
numConstraints=['placement/complement'])
graph.setConstraints(edge='approach-ball',
numConstraints=['placement/complement'])
## 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)
ps.setConstantRightHandSide ('placement/complement', False)
## Set constraints of nodes and edges
graph.setConstraints (node='placement', constraints = \
Constraints (numConstraints = ['placement'],))
graph.setConstraints (node='grasp',
constraints = Constraints (numConstraints = ['grasp']))
graph.setConstraints (edge='transit', constraints = \
Constraints (numConstraints = ['placement/complement']))
graph.setConstraints (edge='grasp-ball', constraints = \
Constraints (numConstraints = ['placement/complement']))
# These edges are in node 'grasp'
graph.setConstraints (edge='transfer', constraints = Constraints ())
graph.setConstraints (edge='release-ball', constraints = Constraints ())
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
graph.initialize ()
## Project initial configuration on state 'placement'
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
_["move_both"] = "move both"
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["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',
constraints = Constraints (numConstraints=['box_placement']))
# Right hand {{{4
cg.setConstraints (node='right',
constraints = Constraints (grasps = ['r_grasp',]))
cg.createWaypointEdge ('free', 'right', 'r_grasp', 1, 10, 'free', True)
cg.setConstraints (edge='r_grasp_e1',
constraints = Constraints (lockedJoints = lockbox))
cg.setConstraints (node='r_grasp_n0',
constraints = Constraints (pregrasps = ['r_pregrasp',]))
cg.setConstraints (edge='r_grasp_e0',
constraints = Constraints (lockedJoints = lockbox))
# 4}}}
robot.setCurrentConfig(q_init)
tf = robot.getJointPosition('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \
#('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]
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)
robot.setCurrentConfig (q_init)
tf = robot.getJointPosition ('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \
#('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]
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])
graph.setConstraints (edge='d2l_e0', numConstraints = bothfeet[1])
graph.setConstraints (edge='d2l_e1', numConstraints = leftfoot[1])
graph.setConstraints (edge='l2d', numConstraints = leftfoot[1])
graph.setConstraints (edge='l2d_ls', numConstraints = leftfoot[1])
graph.setConstraints (edge='l2l', numConstraints = leftfoot[1])
graph.setConstraints (edge='d2r_e0', numConstraints = bothfeet[1])
graph.setConstraints (edge='d2r_e1', numConstraints = rightfoot[1])
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild('graph', grippers, boxes,
handlesPerObject, contactPerObject,
envSurfaces, rules)
cg = ConstraintGraph(robot, 'graph', makeGraph=False)
cg.setConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight('pr2/l_gripper < box/handle | 0-0_ls', 0)
cg.setWeight('Loop | f', 1)
cg.initialize()
res, q_init, err = cg.applyNodeConstraints('free', q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints('free', q_goal)
if not res:
raise RuntimeError("Failed to project initial configuration")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setMaxIterPathPlanning(5000)
import datetime as dt
]
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.setObjects([obj.name for obj in objects],
[obj.handles for obj in objects],
[obj.contacts for obj in objects])
factory.environmentContacts([])
factory.setRules(rules)
factory.generate()
for edgename, edgeid in cg.edges.iteritems():
cg.addConstraints(
edge=edgename,
constraints=Constraints(numConstraints=["waist orientation"]))
cg.setConstraints (graph=True,
constraints = Constraints \
(numConstraints = quasiStaticConstraints + ['gaze'],
lockedJoints = left_gripper_lock + right_gripper_lock +\
other_lock))
for e in cg.edges.keys():
if cg.getWeight(e) == -1:
continue
if e[:4] == "Loop":
cg.setWeight(e, 1)
else:
cg.setWeight(e, 5)
cg.initialize()
n_init = 'table/gripper1 grasps box/handle1'
n_goal = 'table/gripper1 grasps box/handle2'
q_goal = q_init[::]
[False, False, True, True, True, False,])
# translation accross z while gripper is above the box
ps.createTransformationConstraint ('Box', '', gripperName,
[0,0,0.15,0,0,1,0],
[True, True, False, True, True, True])
# Setting up of constraints
ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('Box',False)
#Define Edges constraints
graph.setConstraints (edge='transit', numConstraints = ['placement/complement'])
graph.setConstraints (edge='approach-ball',numConstraints = ['placement/complement'])
graph.setConstraints (edge='move-gripper-away', numConstraints = ['placement/complement'])
graph.setConstraints (edge='grasp-ball', numConstraints = ['Box'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = [])
graph.setConstraints (edge='approach-ground', numConstraints = [])
graph.setConstraints (edge='take-ball-away', numConstraints = [])
graph.setConstraints (edge='take-ball-up', numConstraints = ['Box', 'grasp'])
graph.setConstraints (edge='put-ball-down', numConstraints = ['Box','grasp'])
#Define Nodes constraints
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (node='ball-above-ground', numConstraints = ['ball-above-ground', 'grasp'])
graph.setConstraints (node='grasp-placement', numConstraints = ['grasp', 'placement'])
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])
graph.setConstraints(edge='double', numConstraints=bothfeet[1])
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])
for i in xrange(K):
handlesPerObject.append([boxes[i] + "/handle2"])
handles.append(boxes[i] + "/handle2")
shapes.append([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule([".*"], [".*"], True)]
ps.client.manipulation.graph.autoBuild("graph", grippers, boxes,
handlesPerObject, shapes,
['table/pancake_table_table_top'],
rules)
# Get the built graph
cg = ConstraintGraph(robot, 'graph', makeGraph=False)
cg.setConstraints(graph=True, constraints=Constraints(lockedJoints=lockAll))
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)
import datetime as dt
lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock')
# 4}}}
# Create gaze constraints {{{4
ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
# 4}}}
# 3}}}
# Create the graph. {{{3
# cg.createNode (['r2', 'r1', '2on1', 'free'])
cg.createNode (['r2', 'r1', 'free'])
cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table'])
# cg.setConstraints (node='2on1', numConstraints=['box1_on_table','box2_on_box1'])
cg.setConstraints (node='r1', grasps = ['r1_grasp',])
cg.setConstraints (node='r2', grasps = ['r2_grasp',])
def genGrasp (iBox, lMovingBox, lFixedBox, reuseBoxPositionFactor = 10, newBoxPositionFactor = 1):
iStr = str(iBox)
rN = 'r' + iStr
edgeBN = rN+'_grasp'
pcN = 'box' + iStr + "_on_table"
ppcN = 'pre_box' + iStr + "_on_table"
# Free to right
cg.createWaypointEdge ('free', rN, edgeBN , nb=3, weight=1, automaticBuilder=True)
cg.setConstraints (edge=edgeBN + '_e0', lockDof = lMovingBox + lFixedBox)
lockCup = ps.lockFreeFlyerJoint (Cup.joint, 'cup_lock')
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
# 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}}}
# 2}}}
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
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)
from hpp.gepetto import PathPlayer
