grasps.remove(('r1/gripper', 'cylinder0/handle'))
nodes.append(StateName(grasps))
rules.append(makeRule(grasps=grasps))
exploreNodes.append(nodes[-2])
cg = ConstraintGraph(robot, 'assembly')
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
#factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects(objects, handlesPerObjects, shapesPerObject)
factory.setRules(rules)
factory.generate()
cg.initialize()
edges, loops = getEdges(graph=cg, nodes=nodes, exploreNodes=exploreNodes)
ps.selectPathProjector('Progressive', .05)
## Add a node to move robots in initial configurations
nodes.append(nodes[-1])
edges.append(
getTransitionConnectingStates(graph=cg, s1=nodes[-2], s2=nodes[-1]))
loops.append(
getTransitionConnectingStates(graph=cg, s1=nodes[-1], s2=nodes[-1]))
exploreNodes.append(nodes[-1])
def generateSubGoals(q0, edges):
'''
Generate a list of subgoals
Input:
grasps.remove (('r1/gripper', 'cylinder0/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
cg = ConstraintGraph (robot, 'assembly')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
#factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects (objects, handlesPerObjects, shapesPerObject)
factory.setRules (rules)
factory.generate ()
cg.initialize ()
edges, loops = getEdges (graph = cg, nodes = nodes, exploreNodes = exploreNodes)
ps.selectPathProjector ('Progressive', .05)
## Add a node to move robots in initial configurations
nodes.append (nodes [-1])
edges.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-2],
s2 = nodes [-1]))
loops.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-1],
s2 = nodes [-1]))
exploreNodes.append (nodes [-1])
def generateSubGoals (q0, edges):
'''
Generate a list of subgoals
Input: