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']
# objContactSurfaces = [['box/box_surface']]
envSurfaces = []
objContactSurfaces = [[]]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envSurfaces)
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('Loop | f', 1)
cg.setWeight ('Loop | 0-0', 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")
['box/handle', 'box/handle2'],
]
objContactSurfaces = [
['box/box_surface'],
]
envSurfaces = [
'kitchen_area/pancake_table_table_top',
]
# Build rules
rules = [Rule([""], [""], True)]
cg = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.environmentContacts(envSurfaces)
factory.setObjects(boxes, handlesPerObject, objContactSurfaces)
factory.setRules(rules)
factory.generate()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = lockAll))
cg.initialize()
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 = res[1]
res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_goal)
if not res[0]:
handlesPerObject = list()
handles = list()
objContactSurfaces = list()
for i in range(K):
handlesPerObject.append([boxes[i] + "/handle2"])
handles.append(boxes[i] + "/handle2")
objContactSurfaces.append([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule([".*"], [".*"], True)]
# Get the built graph
cg = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.environmentContacts(['table/pancake_table_table_top'])
factory.setObjects(boxes, handlesPerObject, objContactSurfaces)
factory.setRules(rules)
factory.generate()
cg.addConstraints (graph = True, constraints =\
Constraints (numConstraints = lockAll))
cg.initialize()
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)
handlesPerObject = list ()
handles = list ()
objContactSurfaces = list ()
for i in xrange(K):
handlesPerObject.append ([boxes[i] + "/handle2"])
handles.append (boxes[i] + "/handle2")
objContactSurfaces .append ([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule ([".*"], [".*"], True)]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = lockAll))
cg.initialize ()
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.')
Rule(grippers=grippers, handles=['', 'box/handle2', ''], link=True),
Rule(grippers=grippers,
handles=['', 'box/handle2', 'box/handle1'],
link=True),
Rule(grippers=grippers, handles=['', '', 'box/handle1'], link=True),
Rule(grippers=grippers,
handles=['box/handle2', '', 'box/handle1'],
link=True),
Rule(grippers=grippers, handles=['box/handle2', '', ''], link=True)
]
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":