def addAlignmentConstrainttoEdge(ps, handle, graph):
#recover id of handle
handleId = all_handles.index(handle)
J1, gripperPose = ps.robot.getGripperPositionInJoint(tool_gripper)
J2, handlePose = ps.robot.getHandlePositionInJoint(handle)
T1 = Transform(gripperPose)
T2 = Transform(handlePose)
constraintName = handle + '/alignment'
ps.client.basic.problem.createTransformationConstraint2\
(constraintName, J1, J2, T1.toTuple(), T2.toTuple(),
[False, True, True, False, False, False])
# Set constraint
edgeName = tool_gripper + ' > ' + handle + ' | 0-0_12'
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
edgeName = tool_gripper + ' < ' + handle + ' | 0-0:1-{}_21'.format(handleId)
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
'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
## Create static stability constraint
robot.leftAnkle = robot.robotNames[0] + '/' + robot.leftAnkle
robot.rightAnkle = robot.robotNames[0] + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig()
r = placard.rank
q[r:r + 3] = [.4, 0, 1.2]
ps.addPartialCom("romeo", ["romeo/root_joint"])
robot.createStaticStabilityConstraint('balance/', 'romeo', robot.leftAnkle,
robot.rightAnkle, q)
balanceConstraints = [
'balance/pose-left-foot',
'balance/pose-right-foot',
'balance/relative-com',
]
commonConstraints = Constraints (numConstraints = balanceConstraints + \
lockHands)
# build graph
rules = [
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", ""], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["", "placard/high"], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", "placard/high"], True),
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_["move_free"]="move_free"
_["r_keep_grasp"]="r_keep_grasp"
_["l_keep_grasp"]="l_keep_grasp"
# 4}}}
cg.setTextToTeXTranslation (_)
cg.createNode (['both', 'right', 'left', 'free'])
cg.setConstraints (node='free',
constraints = Constraints (numConstraints=['box_placement']))
# Right hand {{{4
cg.setConstraints (node='right',
constraints = Constraints (grasps = ['r_grasp',]))
cg.createWaypointEdge ('free', 'right', 'r_grasp', 1, 10, 'free', True)
cg.setConstraints (edge='r_grasp_e1',
constraints = Constraints (lockedJoints = lockbox))
cg.setConstraints (node='r_grasp_n0',
constraints = Constraints (pregrasps = ['r_pregrasp',]))
cg.setConstraints (edge='r_grasp_e0',
constraints = Constraints (lockedJoints = lockbox))
# 4}}}
'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
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)
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
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)
ps.addPathOptimizer("RandomShortcut")
createQuasiStaticEquilibriumConstraint (ps, init_conf)
gaze_constraints = createGazeConstraints (ps)
gaze_cost = createGazeCost (ps)
waist_constraint = createWaistYawConstraint (ps)
left_arm_lock = createLeftArmLockedJoints (ps)
right_arm_lock = createRightArmLockedJoints (ps)
left_gripper_lock, right_gripper_lock = \
createGripperLockedJoints (ps, init_conf)
table_lock = createTableLockedJoint (ps, table, init_conf)
graph, factory = makeGraph(ps, table, objects)
# Add other locked joints in the edges.
for edgename, edgeid in graph.edges.items():
graph.addConstraints(
edge=edgename, constraints=Constraints(numConstraints=table_lock)
)
# Add gaze and and COM constraints to each node of the graph
if comConstraint:
for nodename, nodeid in graph.nodes.items():
graph.addConstraints(
node=nodename, constraints=Constraints(numConstraints=\
com_constraint
)
)
# Add locked joints and foot placement constraints in the graph,
# add foot placement complement in each edge.
if footPlacement:
for edgename, edgeid in graph.edges.items():
graph.addConstraints(
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(
edge='transit',
"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(numConstraints=locklhand))
cg.initialize()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
# uncomment to solve
# ps.solve()
# Path optimization uncomment to optimize
#
# ps.loadPlugin('manipulation-spline-gradient-based.so')
# ps.addPathOptimizer('SplineGradientBased_bezier1')
# ps.optimizePath(0)
# display in gepetto-gui
[0, 0, 0.225, 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 ('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 (edge='transit' , constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='approach-ball' , constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='move-gripper-away', constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='move-gripper-up' , constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='grasp-ball' , constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='take-ball-up' , constraints = Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='put-ball-down' , constraints = Constraints (numConstraints = ['placement/complement']))
# These edges are in node 'grasp'
graph.addConstraints (edge='transfer' , constraints = Constraints ())
for i in xrange(K):
handlesPerObject.append([boxes[i] + "/handle2"])
handles.append(boxes[i] + "/handle2")
shapes.append([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule([".*"], [".*"], True)]
ps.client.manipulation.graph.autoBuild("graph", grippers, boxes,
handlesPerObject, shapes,
['table/pancake_table_table_top'],
rules)
# Get the built graph
cg = ConstraintGraph(robot, 'graph', makeGraph=False)
cg.setConstraints(graph=True, constraints=Constraints(lockedJoints=lockAll))
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)
if not res[0]:
raise Exception('Goal configuration could not be projected.')
q_goal_proj = res[1]
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
import datetime as dt
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()
], 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)
'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)
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()
val = v;
ps.createLockedJoint ('romeo/' + j, robot.robotNames [0] + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.robotNames [0] + '/' + robot.leftAnkle
robot.rightAnkle = robot.robotNames [0] + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig ()
r = placard.rank
q [r:r+3] = [.4, 0, 1.2]
ps.addPartialCom ("romeo", ["romeo/root_joint"])
robot.createStaticStabilityConstraint ('balance/', 'romeo', robot.leftAnkle,
robot.rightAnkle, q)
commonConstraints = Constraints (numConstraints = ['balance/pose-left-foot',
'balance/pose-right-foot',
'balance/relative-com'],
lockedJoints = lockHands)
# build graph
rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values ()]
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
handlesPerObjects,
'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
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)
## 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)
right_gripper_lock.append(n)
elif n.startswith("talos/gripper_left"):
ps.createLockedJoint(n, n, init_conf[r:r + s])
left_gripper_lock.append(n)
elif n in table_lock:
ps.createLockedJoint(n, n, init_conf[r:r + s])
ps.setConstantRightHandSide(n, False)
# Set robot to neutral configuration before building constraint graph
robot.setCurrentConfig(q_neutral)
graph = makeGraph(robot, table, objects)
# Add other locked joints in the edges.
for edgename, edgeid in graph.edges.iteritems():
graph.addConstraints(edge=edgename,
constraints=Constraints(numConstraints=table_lock))
# Add gaze and and COM constraints to each node of the graph
if comConstraint:
for nodename, nodeid in graph.nodes.iteritems():
graph.addConstraints(
node=nodename,
constraints=Constraints(
numConstraints=["balance/relative-com", "gaze"]))
# Add locked joints and foot placement constraints in the graph,
# add foot placement complement in each edge.
if footPlacement:
for edgename, edgeid in graph.edges.iteritems():
graph.addConstraints(
edge=edgename,
constraints=Constraints(numConstraints=foot_placement_complement),
0,
0,
0,
0,
0,
1,
]
# Set robot to neutral configuration before building constraint graph
robot.setCurrentConfig(q_neutral)
graph = makeGraph(robot, table, objects)
# Add other locked joints in the edges.
for edgename, edgeid in graph.edges.iteritems():
graph.addConstraints(edge=edgename,
constraints=Constraints(lockedJoints=table_lock))
# Add gaze and and COM constraints to each node of the graph
if comConstraint:
for nodename, nodeid in graph.nodes.iteritems():
graph.addConstraints(
node=nodename,
constraints=Constraints(numConstraints=["com_talos", "gaze"]))
# Add locked joints and foot placement constraints in the graph,
# add foot placement complement in each edge.
if footPlacement:
for edgename, edgeid in graph.edges.iteritems():
graph.addConstraints(
edge=edgename,
constraints=Constraints(numConstraints=foot_placement_complement),
)
"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"]))
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)
val = v
ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig()
r = placard.rank
q[r:r + 3] = [.4, 0, 1.2]
ps.createStaticStabilityConstraints('balance-romeo',
q,
'romeo',
type=ProblemSolver.FIXED_ON_THE_GROUND)
commonConstraints = Constraints(numConstraints=ps.balanceConstraints(),
lockedJoints=lockHands)
# build graph
rules = [
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", ""], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["", "placard/high"], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", "placard/high"], True),
elif n.startswith("talos/gripper_left"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
left_gripper_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r + s])
cg = ConstraintGraph(robot, 'graph')
cg.createNode(['static stability'])
cg.createEdge(nodeFrom='static stability',
nodeTo='static stability',
name='move',
weight=1,
isInNode='static stability')
cg.setConstraints(node='static stability',
constraints=Constraints(
numConstraints=quasiStaticConstraints,
lockedJoints=left_gripper_lock + right_gripper_lock))
cg.initialize()
q_init = half_sitting[::]
q_goal = generateRandomConfig(robot, cg)
ps.setInitialConfig(q_init)
ps.resetGoalConfigs()
ps.addGoalConfig(q_goal)
ps.setParameter('SimpleTimeParameterization/order', 2)
ps.setParameter('SimpleTimeParameterization/maxAcceleration', .5)
ps.setParameter('SimpleTimeParameterization/safety', 0.5)
ps.solve()
pid = ps.numberPaths() - 1
if pid >= 0:
writeTrajInYaml(ps, pid, '/tmp/traj.yaml', .1)