def refresh(self):
self.cg = ConstraintGraph(self.plugin.r, "", False)
try:
self.fillGripper()
self.fillHandles()
self.applyFilters()
except Exception as e:
pass
def makeGraph(robot, table, objects):
graph = ConstraintGraph(robot, "graph")
factory = ConstraintGraphFactory(graph)
grippers = ["talos/left_gripper", "talos/right_gripper"]
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(table.contacts)
factory.constraints.strict = True
factory.setRules (makeRules (robot, grippers))
factory.generate()
return graph
def makeGraph(robot):
from hpp.corbaserver.manipulation.constraint_graph_factory import (
ConstraintGraphFactory,
)
graph = ConstraintGraph(robot, "graph")
factory = ConstraintGraphFactory(graph)
factory.setGrippers(["talos/left_gripper"])
factory.setObjects(["box"], [Object.handles], [Object.contacts])
factory.environmentContacts(Table.contacts)
factory.setRules(
[
Rule(["talos/left_gripper"], [Object.handles[1]], False),
# Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True),
Rule(["talos/left_gripper"], [".*"], True),
# Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True),
]
)
factory.generate()
return graph
def makeGraph(ps, table, objects):
robot = ps.robot
graph = ConstraintGraph(robot, "graph")
factory = ConstraintGraphFactory(graph)
grippers = ["talos/left_gripper", "talos/right_gripper"]
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(table.contacts)
factory.constraints.strict = True
factory.setRules (makeRules (robot, grippers))
factory.setPreplacementDistance("box", 0.1)
factory.generate()
sm = SecurityMargins(ps, factory, ["talos", "box", "table"])
sm.setSecurityMarginBetween ("talos", "box", 0.02)
sm.setSecurityMarginBetween ("box", "table", 0.03)
sm.apply()
return graph, factory
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'
]:
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)
grippers = ['pr2/l_gripper', 'pr2/r_gripper']
boxes = ['box']
handlesPerObject = [
['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]
])
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)
left_arm_lock = createLeftArmLockedJoints(ps)
right_arm_lock = createRightArmLockedJoints(ps)
if args.arm == 'left':
arm_locked = right_arm_lock
elif args.arm == 'right':
arm_locked = left_arm_lock
else:
arm_locked = list()
left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf)
com_constraint, foot_placement, foot_placement_complement = \
createQuasiStaticEquilibriumConstraint (ps, initConf)
gaze_constraint = createGazeConstraint(ps, args.arm)
waist_constraint = createWaistYawConstraint(ps)
graph = ConstraintGraph(robot, "graph")
graph.createNode(["free", "starting_state"])
graph.createEdge("starting_state",
"free",
"starting_motion",
isInNode="starting_state")
graph.createEdge("free",
"starting_state",
"go_to_starting_state",
isInNode="starting_state")
graph.createEdge("free", "free", "Loop | f", isInNode="starting_state")
# Set constraints
graph.addConstraints(
node="starting_state",
