graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
## 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)
[True, True, False, False, False, True,])
# gripper above ball
ps.createTransformationConstraint ('gripper-above-ball', gripperName, ballName,
[0,0.137,0,0.5, 0.5, -0.5, 0.5],
[True, True, True, True, True, True,])
# ball above the ground i.e in the gripper
ps.createTransformationConstraint ('ball-above-ground', '', ballName,
[0,0,0.15,0, 0, 0, 1],
[False, False, True, True, True, False,])
# translation accross z while gripper is above the box
ps.createTransformationConstraint ('Box', '', gripperName,
[0,0,0.15,0,0,1,0],
[True, True, False, True, True, True])
# Setting up of constraints
ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('Box',False)
#Define Edges constraints
graph.setConstraints (edge='transit', numConstraints = ['placement/complement'])
graph.setConstraints (edge='approach-ball',numConstraints = ['placement/complement'])
graph.setConstraints (edge='move-gripper-away', numConstraints = ['placement/complement'])
graph.setConstraints (edge='grasp-ball', numConstraints = ['Box'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = [])
graph.setConstraints (edge='approach-ground', numConstraints = [])
graph.setConstraints (edge='take-ball-away', numConstraints = [])
graph.createGrasp ('grasp', 'ur5/gripper', 'pokeball/handle')
## Create nodes and edges
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')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'placement')
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [False, False, True, True, True, False,])
# Create complement constraint
ps.createTransformationConstraint ('placement/complement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [True, True, False, False, False, True,])
ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False)
## Set constraints of nodes and edges
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (edge='transit',
numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = ['grasp'])
graph.setConstraints (edge='grasp-ball',
numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='release-ball',
numConstraints = ['placement', 'placement/complement'])
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
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"])
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',
graph.createEdge ('grasp', 'ball-above-ground', 'approach-ground', 1, 'grasp')
## 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.createTransformationConstraint ('above-ball', gripperName, ballName, [0, 0.237, 0, 0.5, 0.5, -0.5, 0.5], [True, True, True, True, True, True,])
ps.createTransformationConstraint ('above-ground', '', ballName, [0,0,0.2,0, 0, 0, 1], [False, False, True, True, True, False,])
ps.createTransformationConstraint('z-only', '', gripperName, [0,0,0.2,0,0,1], [True, True, False, True, True, True])
ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('above-ball', True)
ps.setConstantRightHandSide ('above-ground', True)
ps.setConstantRightHandSide ('placement/complement', False)
ps.setConstantRightHandSide ('z-only', False)
## Set constraints of nodes and edges
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='gripper-above-ball', numConstraints = ['placement','above-ball'])
graph.setConstraints (node='grasp-placement', numConstraints = ['grasp', 'placement'])
graph.setConstraints (node='ball-above-ground', numConstraints = ['grasp', 'above-ground'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (edge='transit', numConstraints = ['placement/complement'])
graph.setConstraints (edge='approach-ball', numConstraints = ['placement/complement'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])
ballInGripper, 6 * [
True,
])
ps.createTransformationConstraint('gripper-above-ball', gripperName, ballName,
[0, .3, 0, 0.5, 0.5, -0.5, 0.5], 6 * [
True,
])
ps.createTransformationConstraint('ball-above-ground', '', ballName,
[0, 0, 0.3, 0, 0, 0, 1],
[False, False, True, True, True, False])
ps.createTransformationConstraint('change-in-altitude-only', '', gripperName,
[0, 0, 0.3, 0, 0, 0, 1],
[True, True, False, True, True, True])
ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('change-in-altitude-only', False)
## Set constraints of nodes and edges
graph.setConstraints(node='placement', numConstraints=['placement'])
graph.setConstraints(node='grasp', numConstraints=['grasp'])
graph.setConstraints(node='ball-above-ground',
numConstraints=['grasp', 'ball-above-ground'])
graph.setConstraints(node='grasp-placement',
numConstraints=['grasp', 'placement'])
graph.setConstraints(node='gripper-above-ball',
numConstraints=['gripper-above-ball', 'placement'])