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'
]:
cg.setWeight(e, 100)
for e in [
'ur3/gripper < sphere0/handle | 0-0_ls',
'ur3/gripper < sphere1/handle | 0-1_ls'
]:
cg.setWeight(e, 100)
cg.initialize()
# Run benchmark
#
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
ps.clearRoadmap()
ps.resetGoalConfigs()
# '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")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
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
N = 20
'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 (numConstraints = 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")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setMaxIterPathPlanning(5000)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
success = 0
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[::]
q_goal[-4:] = [0.5, -0.5, 0.5, 0.5]
# Project initial configuration on initial node
res, q_init, err = cg.applyNodeConstraints(n_init, q_init)
if not res: raise RuntimeError('Failed to project initial configuration')
res, q_goal, err = cg.applyNodeConstraints(n_goal, q_goal)
if not res: raise RuntimeError('Failed to project goal configuration')
# Set Gaussian configuration shooter.
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)
totalNumberNodes = 0
