def makeGraph(robot):
graph = ConstraintGraph.buildGenericGraph(
robot,
'graph',
# [ "talos/left_gripper", "talos/right_gripper", "table/pose", ],
[
"talos/left_gripper",
"table/pose",
],
[
"box",
],
[
Object.handles + Object.poses,
],
# [ Object.contacts, ],
[
[],
],
Table.contacts,
[
Rule([
"table/pose",
], [
"box/handle[12]",
], False),
Rule([
"talos/left_gripper",
], [
"box/pose[12]",
], False),
Rule([
"table/pose",
], [
"box/pose1",
], False),
Rule([
"talos/left_gripper",
], [
Object.handles[1],
], False),
Rule([
"talos/left_gripper",
], [
Object.handles[0],
], True),
# Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True),
Rule([
"table/pose",
], [
"box/pose2",
], True),
])
return graph
def createInitializationGraph(name):
graph = ConstraintGraph.buildGenericGraph(
robot,
name,
["talos/left_gripper", "talos/right_gripper"],
["box"],
[Object.handles],
[[]],
[],
[
Rule(["talos/left_gripper"], [Object.handles[0]], True),
Rule(["talos/right_gripper"], [Object.handles[1]], True),
],
)
return graph
0,
1,
]
lang = 'py'
if lang == 'cxx':
rules = [
Rule(grippers, [''], True),
Rule(grippers, ['sphere0/handle'], True),
Rule(grippers, ['sphere1/handle'], True)
]
cg = ConstraintGraph.buildGenericGraph(robot=robot,
name="manipulation",
grippers=grippers,
objects=objects,
handlesPerObjects=handlesPerObject,
shapesPerObjects=contactsPerObject,
envNames=[],
rules=rules)
if lang == 'py':
cg = ConstraintGraph(robot, "manipulation")
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.setObjects(objects, handlesPerObject, contactsPerObject)
factory.generate()
for e in [
'ur3/gripper > sphere0/handle | f_ls',
'ur3/gripper > sphere1/handle | f_ls'
]:
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["", "placard/high"], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", "placard/high"], True),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values()]
cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, [
placard.name,
], handlesPerObjects, [
[],
], [], rules)
# cg = ConstraintGraph (robot, "graph")
# factory = ConstraintGraphFactory (cg)
# factory.setGrippers (grippers)
# factory.setObjects ([placard.name,], handlesPerObjects, [[],])
# factory.setRules (rules)
# factory.generate ()
cg.addConstraints(graph=True, constraints=commonConstraints)
cg.initialize()
# Define initial and final configurations
q_goal = [
shapes = list()
for i in xrange(K):
handles.append([boxes[i] + "/handle2"])
shapes.append([boxes[i] + "/box_surface"])
if K is 2:
rules = [
Rule(grippers[0], handles[1][0], False),
Rule(grippers[1], handles[0][0], False),
]
else:
rules = []
# Build the constraint graph
cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, boxes,
handles, shapes,
['table/pancake_table_table_top'],
rules)
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)
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])
graph = ConstraintGraph.buildGenericGraph(robot, 'graph',
[ "talos/left_gripper", "talos/right_gripper", ],
[ "box", ],
[ Object.handles, ],
[ [ ], ],
[ ],
[ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True),
Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ]
)
graph.setConstraints (graph=True,
lockDof = left_gripper_lock + right_gripper_lock + other_lock,
numConstraints = [ "com_talos_box", "gaze"] + foot_placement)
graph.initialize()
res, q_init, err = graph.applyNodeConstraints("talos/left_gripper grasps box/top", half_sitting)
res, q_goal, err = graph.applyNodeConstraints("talos/right_gripper grasps box/bottom", half_sitting)
print ps.directPath(q_init, q_init, True)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
lockAll = lockFingers + lockHead
# 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}}}
# Build the constraint graph
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
[ "baxter/r_gripper" , "baxter/l_gripper"],
[ "box1" , "box2"],
[["box1/handle2",] , ["box2/handle2"]],
[["box1/box_surface"], ["box2/box_surface"]],
['table/pancake_table_table_top'],
[])
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]
elif n.startswith("talos/gripper_left"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
left_gripper_lock.append(n)
elif n.startswith("talos/head"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
head_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r + s])
graph = ConstraintGraph.buildGenericGraph(
robot,
"graph",
["talos/left_gripper", "talos/right_gripper"],
["mire"],
[Mire.handles],
[[]],
[],
[
Rule(["talos/left_gripper"], [Mire.handles[0]], True),
Rule(["talos/right_gripper"], [Mire.handles[1]], True),
],
)
graph.setConstraints(
graph=True,
lockDof=left_gripper_lock + right_gripper_lock + other_lock,
numConstraints=["com_talos_mire"] + foot_placement,
)
graph.initialize()
commonConstraints = Constraints (numConstraints = ps.balanceConstraints (),
lockedJoints = lockHands)
# build graph
rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values ()]
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
handlesPerObjects,
[[],],
[],
rules)
# cg = ConstraintGraph (robot, "graph")
# factory = ConstraintGraphFactory (cg)
# factory.setGrippers (grippers)
# factory.setObjects ([placard.name,], handlesPerObjects, [[],])
# factory.setRules (rules)
# factory.generate ()
cg.addConstraints (graph = True, constraints = commonConstraints)
cg.initialize ()
# Define initial and final configurations
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead
# 4}}}
# 3}}}
handles = list ()
shapes = list ()
for i in xrange(K):
handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"])
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers, boxes, handles, shapes,
['table/pancake_table_table_top'], [])
# Get the built graph
cg.setConstraints (graph = True, lockDof = lockAll)
# cg.graph.setTargetNodeList (cg.subGraphId,
# [
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box1/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box2/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box3/handle2'],
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])
graph = ConstraintGraph.buildGenericGraph(robot, 'graph', [
"talos/left_gripper",
"talos/right_gripper",
], [
"box",
], [
Object.handles,
], [
[],
], [], [
Rule([
"talos/left_gripper",
], [
Object.handles[0],
], True),
Rule([
"talos/right_gripper",
], [
Object.handles[1],
], True),
])
graph.setConstraints(graph=True,
lockDof=left_gripper_lock + right_gripper_lock +
other_lock,
numConstraints=["com_talos_box", "gaze"] + foot_placement)
ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [
0.5,
])
# 2}}}
# Create the constraint graph. {{{2
graph = ConstraintGraph.buildGenericGraph(robot, "manipulate_box", [
"pr2/l_gripper",
], [
"box",
], [
[
"box/handle2",
],
], [
[
"box/box_surface",
],
], [
"kitchen_area/pancake_table_table_top",
], [
Rule([".*"], [".*"], True),
])
# Allow everything
graph.setConstraints (graph = True, constraints = Constraints \
(lockedJoints = locklhand))
# 2}}}
res, q_init_proj, err = graph.applyNodeConstraints("free", q_init)
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.startswith("talos/head"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
head_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r + s])
graph = ConstraintGraph.buildGenericGraph(
robot,
"graph",
["talos/left_gripper", "talos/right_gripper"],
[],
[],
[], # contacts per object
[], # env contacts
[],
)
graph.setConstraints(
graph=True,
lockDof=left_gripper_lock + right_gripper_lock + other_lock,
numConstraints=[] + foot_placement,
)
graph.initialize()
q_init = half_sitting[:]
