ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
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)
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)
cg.initialize()
exploreNodes.append(nodes[-2])
# release sphere1
grasps.remove(('r0/gripper', 'sphere1/handle'))
nodes.append(StateName(grasps))
rules.append(makeRule(grasps=grasps))
exploreNodes.append(nodes[-2])
# release cylinder0 : put assembly on the ground
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(
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]
# 3}}}
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)
ps.createPositionConstraint("look_at_gripper", "tiago/camera_color_optical_frame", tool_gripper,
(0,0,0), (0,0,0), (True,True,True))
look_at_gripper = ps.hppcorba.problem.getConstraint("look_at_gripper")
import hpp_idl
look_at_gripper.setComparisonType([hpp_idl.hpp.EqualToZero,hpp_idl.hpp.EqualToZero,hpp_idl.hpp.Superior])
# Create "Look at part" constraint
ps.createPositionConstraint("look_at_part", "tiago/camera_color_optical_frame", "part/to_tag_00001",
(0,0,0), (0,0,0), (True,True,False))
look_at_part = ps.hppcorba.problem.getConstraint("look_at_part")
# 3}}}
# {{{3 Constraint graph instanciation
from hpp.corbaserver.manipulation import ConstraintGraphFactory
graph = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(graph)
factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ])
all_handles = ps.getAvailable('handle')
part_handles = filter(lambda x: x.startswith("part/"), all_handles)
test_handles = [ 'part/handle_10',
'part/handle_11',
'part/handle_12',
'part/handle_13',
'part/handle_14',
'part/handle_15',
'part/handle_16',
'part/handle_17',
'part/handle_18',
'part/handle_19',
'part/handle_20',
ps.createRelativeComConstraint("talos_com_constraint_to_la", "Talos_CoM", robot.leftAnkle, com_la.tolist(), (True, True, False))
ps.setMaxIterProjection(40) # Set the maximum number of iterations of the projection algorithm. Must be called before the graph creation.
# constraint graph
rules = [
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_top"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_front"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/upper_drawer_spherical_handle"], True)
]
graph = ConstraintGraph(robot, "graph")
factory = ConstraintGraphFactory(graph)
factory.setGrippers(["talos/left_gripper", "talos/right_gripper"])
factory.setObjects(["desk"], [["desk/upper_drawer_spherical_handle", "desk/touchpoint_right_top", "desk/touchpoint_right_front", "desk/touchpoint_left_front", "desk/touchpoint_left_drawer"]], [[]])
factory.setRules(rules)
factory.generate()
graph.addConstraints(graph = True,
constraints = Constraints(numConstraints = ["talos_com_constraint_to_la", "talos_static_stability/pose-left-foot", "talos_static_stability/pose-right-foot"],
lockedJoints = ["gripper_r_lock_idj", "gripper_r_lock_f1j", "gripper_r_lock_f2j", "gripper_r_lock_isj", "gripper_r_lock_f3j", "gripper_r_lock_j", "gripper_r_lock_msj",
"gripper_l_lock_idj", "gripper_l_lock_f1j", "gripper_l_lock_f2j", "gripper_l_lock_isj", "gripper_l_lock_f3j", "gripper_l_lock_j", "gripper_l_lock_msj"]))
graph.addConstraints(node = "talos/right_gripper grasps desk/touchpoint_right_top", constraints = Constraints(lockedJoints = ["drawer_lock"]))
graph.addConstraints(node = "talos/right_gripper grasps desk/touchpoint_right_front", constraints = Constraints(lockedJoints = ["drawer_lock"]))
graph.addConstraints(node = "talos/left_gripper grasps desk/touchpoint_left_front", constraints = Constraints(lockedJoints = ["drawer_lock"]))
graph.addConstraints(node = "talos/left_gripper grasps desk/touchpoint_left_drawer", constraints = Constraints(lockedJoints = ["drawer_lock"]))
graph.initialize()
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [list(placard.handles.values ())]
lang = 'py'
if lang == 'cxx':
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
handlesPerObjects,
[[],],
[], rules)
if lang == 'py':
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 = [-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]
q_init = q_goal [::]
q_init [r+3:r+7] = [0, 0, 1, 0]
n = 'romeo/l_hand grasps placard/low'
res, q_init, err = cg.applyNodeConstraints (n, q_init)
rules = [
Rule(grippers=grippers, handles=['box/handle1', '', ''], link=True),
Rule(grippers=grippers,
handles=['box/handle1', 'box/handle2', ''],
link=True),
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 +\
ljs = list()
ljs.append(lockJoint("tiago/root_joint", q0))
for n in robot.jointNames:
if n.startswith('tiago/hand_'):
ljs.append(lockJoint(n, q0))
ps.createPositionConstraint("gaze", "tiago/xtion_rgb_optical_frame",
"driller/tag_joint", (0, 0, 0), (0, 0, 0),
(True, True, False))
from hpp.corbaserver.manipulation import ConstraintGraphFactory
graph = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(graph)
factory.setGrippers([
"tiago/gripper",
"driller/drill_tip",
])
factory.setObjects([
"driller",
"skin",
], [
[
"driller/handle",
],
[
"skin/hole",
],
], [
exploreNodes.append (nodes [-2])
# release sphere1
grasps.remove (('r0/gripper', 'sphere1/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# release cylinder0 : put assembly on the ground
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],