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")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[False, False, True, True, True, False,])
# Create complement constraint
ps.createTransformationConstraint ('placement/complement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[True, True, False, False, False, True,])
ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False)
## Set constraints of nodes and edges
graph.addConstraints (node='placement', constraints = \
Constraints (numConstraints = ['placement'],))
graph.addConstraints (node='grasp',
constraints = Constraints (numConstraints = ['grasp']))
graph.addConstraints (edge='transit', constraints = \
Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='grasp-ball', constraints = \
Constraints (numConstraints = ['placement/complement']))
# These edges are in node 'grasp'
graph.addConstraints (edge='transfer', constraints = Constraints ())
graph.addConstraints (edge='release-ball', constraints = Constraints ())
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
graph.initialize ()
## Project initial configuration on state 'placement'
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
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]:
raise Exception('Goal configuration could not be projected.')
q_goal = res[1]
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
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 (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
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",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock +
gaze_constraint + waist_constraint))
graph.addConstraints(edge="Loop | f",
constraints=Constraints(numConstraints=com_constraint +
foot_placement + arm_locked))
graph.initialize()
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
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 (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 = 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.')
q_goal = res[1]
import datetime as dt
'part/handle_20',
'part/handle_4',
'part/handle_5',
'part/handle_9']
factory.setObjects([ "driller", "part", ], [ [ "driller/handle", ], part_handles, ], [ [], [] ])
factory.setRules([
# Forbid driller to grasp itself.
Rule([ "driller/drill_tip", ], [ "driller/handle", ], False),
# Tiago always hold the gripper.
Rule([ "tiago/gripper", ], [ "", ], False), Rule([ "tiago/gripper", ], [ "part/.*", ], False),
# Allow to associate drill_tip with part holes only.
Rule([ "tiago/gripper", "driller/drill_tip", ], [ "driller/handle", ".*", ], True), ])
factory.generate()
graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs))
free = "tiago/gripper grasps driller/handle"
loop_free = 'Loop | 0-0'
for n in graph.nodes.keys():
if n == free: continue
graph.addConstraints(node=n, constraints=Constraints(numConstraints=["look_at_gripper"]))
for e in graph.edges.keys():
graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["tiago_base"]))
#Add alignment constraints
for handle in part_handles:
addAlignmentConstrainttoEdge(ps, handle, graph)
graph.createNode('home', priority=1000)
graph.createEdge('home', 'home', 'move_base')
graph.createEdge('home', free , 'start_arm', isInNode="home")
graph.createEdge( free , 'home', 'end_arm', isInNode=free)
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)
if not res: raise RuntimeError ("Failed to project initial configuration.")
res, q_goal, err = cg.applyNodeConstraints (n, q_goal)
if not res: raise RuntimeError ("Failed to project initial configuration.")
ps.selectPathProjector ("Progressive", .05)
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() # Remove collision pairs def removeTouchpointsCollision(gripper, edge, joint, extendToArmEnd = False): if gripper == "right": graph.removeCollisionPairFromEdge(edge, "talos/gripper_right_fingertip_1_joint", joint) graph.removeCollisionPairFromEdge(edge, "talos/gripper_right_fingertip_2_joint", joint) graph.removeCollisionPairFromEdge(edge, "talos/gripper_right_fingertip_3_joint", joint) graph.removeCollisionPairFromEdge(edge, "talos/gripper_right_inner_single_joint", joint)
True,
True,
False,
False,
False,
True,
],
)
ps.setConstantRightHandSide("placement", True)
ps.setConstantRightHandSide("placement/complement", False)
# Set constraints of nodes and edges
graph.addConstraints(
node="placement",
constraints=Constraints(
numConstraints=["placement"],
),
)
graph.addConstraints(node="grasp", constraints=Constraints(numConstraints=["grasp"]))
graph.addConstraints(
edge="transit", constraints=Constraints(numConstraints=["placement/complement"])
)
graph.addConstraints(
edge="grasp-ball", constraints=Constraints(numConstraints=["placement/complement"])
)
# These edges are in node 'grasp'
graph.addConstraints(edge="transfer", constraints=Constraints())
graph.addConstraints(edge="release-ball", constraints=Constraints())
ps.selectPathValidation("Dichotomy", 0)
ps.selectPathProjector("Progressive", 0.1)
graph.initialize()
True,
False,
False,
False,
True,
])
ps.setConstantRightHandSide('placementBallOnGround', True)
ps.setConstantRightHandSide('placementBallAboveGround', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('placementAboveBall', True)
## Set constraints of nodes and edges
graph.addConstraints(node='placement',
constraints=Constraints(
numConstraints=['placementBallOnGround'], ))
graph.addConstraints(
node='gripper-above-ball',
constraints=Constraints(
numConstraints=['placementBallOnGround', 'placementAboveBall']))
graph.addConstraints(
node='grasp-placement',
constraints=Constraints(numConstraints=['grasp', 'placementBallOnGround']))
graph.addConstraints(node='ball-above-ground',
constraints=Constraints(
numConstraints=['grasp', 'placementBallAboveGround']))
graph.addConstraints(node='grasp',
constraints=Constraints(numConstraints=['grasp']))
graph.addConstraints(
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":
cg.setWeight(e, 1)
else:
cg.setWeight(e, 5)
cg.initialize()
"kitchen_area/pancake_table_table_top",
]
# Define rules for associating grippers and handles (here all associations are
# allowed)
rules = [
Rule([".*"], [".*"], True),
]
cg = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.environmentContacts(envContactSurfaces)
factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject)
factory.setRules(rules)
factory.generate()
cg.addConstraints (graph = True, constraints = Constraints \
(lockedJoints = locklhand))
cg.initialize()
# 2}}}
res, q_init_proj, err = cg.applyNodeConstraints("free", q_init)
res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal)
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
# print ps.solve()
# ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"])
# print ps.solve()
], False),
# Allow to associate drill_tip with skin/hole only.
Rule([
"driller/drill_tip",
], [
"driller/handle",
], False),
Rule([
"driller/drill_tip",
], [
".*",
], True),
])
factory.generate()
graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs))
for n in [
'driller/drill_tip > skin/hole | 0-0_pregrasp',
'tiago/gripper grasps driller/handle : driller/drill_tip grasps skin/hole'
]:
graph.addConstraints(node=n,
constraints=Constraints(numConstraints=["gaze"]))
graph.initialize()
# Constraint in this state are explicit so ps.setMaxIterProjection(1) should not
# make it fail.
res, q1, err = graph.applyNodeConstraints(
'tiago/gripper grasps driller/handle', q0)
q1valid, msg = robot.isConfigValid(q1)
if not q1valid:
print(msg)
