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):
graph = ConstraintGraph.buildGenericGraph(
robot,
'graph',
# [ "talos/left_gripper", "talos/right_gripper", "table/pose", ],
[
"talos/left_gripper",
"table/pose",
],
[
"box",
],
[
Object.handles + Object.poses,
],
# [ Object.contacts, ],
[
[],
],
Table.contacts,
[
Rule([
"table/pose",
], [
"box/handle[12]",
], False),
Rule([
"talos/left_gripper",
], [
"box/pose[12]",
], False),
Rule([
"table/pose",
], [
"box/pose1",
], False),
Rule([
"talos/left_gripper",
], [
Object.handles[1],
], False),
Rule([
"talos/left_gripper",
], [
Object.handles[0],
], True),
# Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True),
Rule([
"table/pose",
], [
"box/pose2",
], True),
])
return graph
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 createInitializationGraph(name):
graph = ConstraintGraph.buildGenericGraph(
robot,
name,
["talos/left_gripper", "talos/right_gripper"],
["box"],
[Object.handles],
[[]],
[],
[
Rule(["talos/left_gripper"], [Object.handles[0]], True),
Rule(["talos/right_gripper"], [Object.handles[1]], True),
],
)
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
vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 0, 0, 0, 1])
vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 0, 0, 0, 1])
# Loading our pokeball which plays our ball's role
vf.loadObjectModel (Pokeball, 'pokeball')
robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,1.,
-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
r = vf.createViewer ()
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
r (q1)
## Create graph
graph = ConstraintGraph (robot, 'graph')
# Contraint of ball relative position while it is located in the gripper
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
# We are creating our graph as described in the instructions
graph.createNode (['grasp-placement', 'ball-above-ground','gripper-above-ball','grasp','placement'])
graph.createEdge ('grasp-placement','ball-above-ground','take-ball-up',1,'grasp')
graph.createEdge ('ball-above-ground','grasp-placement','put-ball-down',1,'grasp')
graph.createEdge ('grasp-placement','gripper-above-ball','move-gripper-up',1,'placement')
s = robot.getJointConfigSize(n)
r = robot.rankInConfiguration[n]
if n.startswith ("talos/gripper_right"):
ps.createLockedJoint(n, n, half_sitting[r:r+s])
right_gripper_lock.append(n)
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])
graph = ConstraintGraph.buildGenericGraph(robot, 'graph',
[ "talos/left_gripper", "talos/right_gripper", ],
[ "box", ],
[ Object.handles, ],
[ [ ], ],
[ ],
[ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True),
Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ]
)
graph.setConstraints (graph=True,
lockDof = left_gripper_lock + right_gripper_lock + other_lock,
numConstraints = [ "com_talos_box", "gaze"] + foot_placement)
graph.initialize()
res, q_init, err = graph.applyNodeConstraints("talos/left_gripper grasps box/top", half_sitting)
res, q_goal, err = graph.applyNodeConstraints("talos/right_gripper grasps box/bottom", half_sitting)
print ps.directPath(q_init, q_init, True)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
lockAll = lockhands + lockHeadAndTorso + lockPlanar
# 3}}}
# Create the graph. {{{3
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, 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)
rank = robot.rankInConfiguration ['box/root_joint']
q_goal [rank:rank+7] = [-2.5, -4.4, 0.76,0,-c,0,c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterProjection (40)
ps.selectPathProjector ('Progressive', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint \
('box_placement', ['box/box_surface',],
['kitchen_area/pancake_table_table_top',])
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
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",
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']
# 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)
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)
rank = robot.rankInConfiguration ['box2/base_joint_xyz']
q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386]
q2[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
#ps.selectPathValidation ('Graph-Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.1)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxter'] = list ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
jointNames['baxter'].append (n)
if n.startswith ("baxter/left_"):
jointNames['baxterLeftSide'].append (n)
if n.startswith ("baxter/right_"):
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead
# 4}}}
# 3}}}
handles = list ()
shapes = list ()
for i in xrange(K):
handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"])
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers, boxes, handles, shapes,
['table/pancake_table_table_top'], [])
# Get the built graph
cg.setConstraints (graph = True, lockDof = lockAll)
# cg.graph.setTargetNodeList (cg.subGraphId,
# [
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box1/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box2/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box3/handle2'],
rank = robot.rankInConfiguration ['cup/base_joint_xyz']
q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
q_goal = q_init [::]
q_goal [rank:rank+7] = [-4.8, -3.35, 0.9, 0,sqrt(2)/2,sqrt(2)/2,0]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# ps.selectPathProjector ('Progressive', 0.2)
ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['romeo'] = list ()
jointNames['romeoWithoutLeftArm'] = list ()
jointNames['romeoWithoutRightArm'] = list ()
jointNames['kitchen_area'] = list ()
jointNames['cup'] = list ()
for n in jointNames['all']:
if n.startswith ("romeo"):
jointNames['romeo'].append (n)
if not n.startswith ("romeo/l_"):
jointNames['romeoWithoutLeftArm'].append (n)
if not n.startswith ("romeo/r_"):
# Create the constraint graph. {{{2
# Define the set of grippers used for manipulation
grippers = [ "pr2/l_gripper", ]
# Define the set of objects that can be manipulated
objects = [ "box", ]
# Define the set of handles for each object
handlesPerObject = [ ["box/handle2", ], ]
# Define the set of contact surfaces used for each object
contactSurfacesPerObject = [ ["box/box_surface", ], ]
# Define the set of contact surfaces of the environment used to put objects
envContactSurfaces = [ "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)
del c
q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver (robot)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint (
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top')
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])
q_init = half_sitting [::]
# Open hands
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6']
q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz']
q_goal = q_init [::]
q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
#q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}}
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')
robot.setCurrentConfig (q_init)
leftpos = robot.getJointPosition (robot.leftAnkle)
rightpos = robot.getJointPosition (robot.rightAnkle)
leftfoot = [ [
# "leftfoot/comz" ,
"leftfoot/com", "leftfoot/z" , "leftfoot/rxry"],
["leftfoot/xy",
]]
# "leftfoot/rxryrz"]]
ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True))
ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True))
ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False)
ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True))
q_init = half_sitting[::]
# Open hands
ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6']
q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration['hrp2/RARM_JOINT6']
q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz']
q_goal = q_init[::]
q_goal[ibjxyz:ibjxyz + 2] = [0.5, 0]
#q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}}
# Generate constraints {{{3
graph = ConstraintGraph(robot, 'graph')
robot.setCurrentConfig(q_init)
leftpos = robot.getJointPosition(robot.leftAnkle)
rightpos = robot.getJointPosition(robot.rightAnkle)
leftfoot = [
[
# "leftfoot/comz" ,
"leftfoot/com",
"leftfoot/z",
"leftfoot/rxry"
],
[
"leftfoot/xy",
]
]
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),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values ()]
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
handlesPerObjects,
[[],],
[],
rules)
# 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
-0.19634763254425533, -0.7205092487114027, 0.6650461296003519,
0.005260724207565836
]
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.selectPathOptimizer('None')
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)
from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver(robot)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint(
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface',
'pancake_table_table_top')
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("pr2"):
jointNames['pr2'].append(n)
if not n.startswith("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append(n)
# 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 = [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
lockrhand = ['r_l_finger','r_r_finger'];
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")
com_la = tf_la.inverse().transform(com_wf)
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"]))
])
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 (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)
lockjoints = list()
lockjoints.extend(lockbox)
lockjoints.extend(lockur5)
robot.setCurrentConfig(q_init)
tf = robot.getJointPosition('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \
#('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True))
robot.client.basic.problem.createPositionConstraint \
('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True))
# 3}}}
# Create the graph. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
cg.createNode(['free'])
cg.setConstraints(node='free', numConstraints=['pos'])
cg.setConstraints(graph=True, lockDof=lockjoints)
cg.createEdge('free', 'free', 'move_free', 1)
# 3}}}
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)
q_init = half_sitting [::]
# Open hands
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6']
q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz']
q_goal = q_init [::]
q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
# q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}}
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')
robot.setCurrentConfig (q_init)
leftpos = robot.getJointPosition (robot.leftAnkle)
rightpos = robot.getJointPosition (robot.rightAnkle)
leftfoot = [ [
# "leftfoot/comz" ,
"leftfoot/com", "leftfoot/z" , "leftfoot/rxry"],
["leftfoot/xy",
]]
# "leftfoot/rxryrz"]]
ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True))
ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True))
ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False)
ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True))
ps = ProblemSolver(robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## fk.createRealClient ()
fk = ViewerFactory(ps)
fk.loadObjectModel(Box, 'box')
fk.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/base_joint_xy", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/base_joint_xyz", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph(robot, 'graph')
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
jointNames = dict()
jointNames['all'] = robot.getJointNames()
from hpp.corbaserver.manipulation import ConstraintGraph, Constraints
from hpp.corbaserver import Client
Client ().problem.resetProblem ()
from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadObjectModel (Pokeball, 'pokeball')
robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,2.,
-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
## Create constraint graph
graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
elif n.startswith("talos/gripper_left"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
left_gripper_lock.append(n)
elif n.startswith("talos/head"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
head_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r + s])
graph = ConstraintGraph.buildGenericGraph(
robot,
"graph",
["talos/left_gripper", "talos/right_gripper"],
["mire"],
[Mire.handles],
[[]],
[],
[
Rule(["talos/left_gripper"], [Mire.handles[0]], True),
Rule(["talos/right_gripper"], [Mire.handles[1]], True),
],
)
graph.setConstraints(
graph=True,
lockDof=left_gripper_lock + right_gripper_lock + other_lock,
numConstraints=["com_talos_mire"] + foot_placement,
)
graph.initialize()
])
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)
lockAll = lockFingers + lockHead
# 4}}}
# Create gaze constraints {{{4
ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
# 4}}}
# 3}}}
# Build the constraint graph
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
[ "baxter/r_gripper" , "baxter/l_gripper"],
[ "box1" , "box2"],
[["box1/handle2",] , ["box2/handle2"]],
[["box1/box_surface"], ["box2/box_surface"]],
['table/pancake_table_table_top'],
[])
cg.setConstraints (graph = True, lockDof = 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]
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
ps.selectPathProjector ('Progressive', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint (
'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top')
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames ['pr2'])
lockjoints = list ()
lockjoints.extend (lockbox)
lockjoints.extend (lockur5)
robot.setCurrentConfig (q_init)
tf = robot.getJointPosition ('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \
#('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True))
robot.client.basic.problem.createPositionConstraint \
('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True))
# 3}}}
# Create the graph. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
cg.createNode (['free'])
cg.setConstraints (node = 'free', numConstraints = ['pos'])
cg.setConstraints (graph = True, lockDof = lockjoints)
cg.createEdge ('free', 'free', 'move_free', 1)
# 3}}}
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)
shapes = list()
for i in xrange(K):
handles.append([boxes[i] + "/handle2"])
shapes.append([boxes[i] + "/box_surface"])
if K is 2:
rules = [
Rule(grippers[0], handles[1][0], False),
Rule(grippers[1], handles[0][0], False),
]
else:
rules = []
# Build the constraint graph
cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, boxes,
handles, shapes,
['table/pancake_table_table_top'],
rules)
cg.setConstraints(graph=True, lockDof=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)
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, [
[],
], [], rules)
# 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.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5,
0.02916333341652683, 0.025597731231524482, 0.16145134862579935,
-2.785939904956431, -4.563075760833335, 0.8958690128585236,
-0.19634763254425533, -0.7205092487114027, 0.6650461296003519,
0.005260724207565836
]
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)
ps.selectPathProjector('Progressive', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint(
'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top')
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("pr2"):
jointNames['pr2'].append(n)
if not n.startswith("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append(n)
ps.addPassiveDofs('pr2', jointNames['pr2'])
for j, v in robot.openHand (None, .24, "left").iteritems () :
lockedJointName = "close/"+j
closeLeftHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
closeRightHand = []
for j, v in robot.openHand (None, .24, "right").iteritems () :
lockedJointName = "close/"+j
closeRightHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')
lockBase = ps.lockPlanarJoint ('tom/base_joint', 'base_lock', [0,0,1,0])
# Build constraint graph
graph = ConstraintGraph (robot, 'graph')
graph.createNode (['hold_box_l1', 'hold_box_l2', 'hold_box_r1', 'hold_box_r2',
'free'])
jointNames = dict ()
jointNames ['tom'] = list ()
for j in robot.jointNames:
if j.startswith ('tom'):
jointNames ['tom'].append (j)
ps.addPassiveDofs ('tom', jointNames ['tom'])
graph.createGrasp ('l2_grasp', 'tom/l_finger_tip', 'box/handle2',
passiveJoints = 'tom')
graph.createGrasp ('l1_grasp', 'tom/l_finger_tip', 'box/handle',
passiveJoints = 'tom')
b1 = q_goal_inverted[rankB1:rankB1+7]
q_goal_inverted[rankB1:rankB1+7] = q_goal_inverted[rankB2:rankB2+7]
q_goal_inverted[rankB2:rankB2+7] = b1
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
ps.selectPathValidation ('Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.1)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxter'] = list ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
jointNames['baxter'].append (n)
if n.startswith ("baxter/left_"):
jointNames['baxterLeftSide'].append (n)
if n.startswith ("baxter/right_"):
tool_gripper = "driller/drill_tip"
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',
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]
shapes = list ()
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, lockDof = 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)
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
fk = FakeViewer (ps)
fk.loadObjectModel (Box, 'box')
fk.buildCompositeRobot (['pr2', 'box'])
fk.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph (robot, 'graph')
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
jointNames = dict ()
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Set initial position of screw-driver
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
# Open left hand
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
q_goal = q_init [::]
q_goal [-7:] = [2, -1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
# 3}}}
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("hrp2"):
jointNames['hrp2'].append (n)
if not n.startswith ("hrp2/LARM"):
jointNames['allButHRP2LeftArm'].append (n)
ps.addPassiveDofs ('hrp2', jointNames ['hrp2'])
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
0,
1,
]
lang = 'py'
if lang == 'cxx':
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'
]:
1.0001,
-1.0001,
1.0001,
-1.0001,
1.0001,
-1.0001,
1.0001,
])
r = vf.createViewer()
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
r(q1)
## Create graph
graph = ConstraintGraph(robot, 'graph')
ps.selectPathValidation("Discretized", 0.01)
ps.selectPathProjector("Progressive", 0.1)
graph.initialize()
res, q_init, error = graph.applyNodeConstraints('placement', q1)
q2 = q1[::]
q2[7] = .2
res, q_goal, error = graph.applyNodeConstraints('placement', q2)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
pp = PathPlayer(ps.client.basic, r)
