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')
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],
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')
graph.createEdge ('gripper-above-ball','grasp-placement','grasp-ball',1,'placement')
graph.createEdge ('gripper-above-ball','placement','move-gripper-away',1,'placement')
graph.createEdge ('placement','gripper-above-ball','approach-ball',1,'placement')
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('ball-above-ground','grasp','take-ball-away',1,'grasp')
graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
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',
)
q1 = [0, -1.57, 1.57, 0, 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, 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")
graph.createEdge("grasp", "placement", "release-ball", 1, "grasp")
graph.createEdge('gripper-above-ball', 'grasp-placement', 'grasp-ball', 1,
'placement')
graph.createEdge('grasp-placement', 'gripper-above-ball', 'move-gripper-up', 1,
'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('ball-above-ground', 'ball-above-ground', 'move-ball', 1,
'grasp')
graph.createEdge('ball-above-ground', 'grasp', 'take-ball-away', 1, 'grasp')
graph.createEdge('grasp', 'ball-above-ground', 'approach-ground', 1, 'grasp')
graph.createEdge('grasp', 'grasp', 'transfer', 1, 'grasp')
ps.createTransformationConstraint('grasp', gripperName, ballName,
ballInGripper, 6 * [
True,
])
ps.createTransformationConstraint('placementAboveBall', gripperName, ballName,
ballUnderGripper, 6 * [
True,
])
## Create transformation constraint : ball is in horizontal plane with free
# rotation around z
ps.createTransformationConstraint('placementBallOnGround', '', ballName,
[0, 0, 0.025, 0, 0, 0, 1], [
False,
False,
True,
True,
True,
graph.createEdge('gripper-above-ball', 'grasp-placement', 'grasp-ball', 1,
'placement')
graph.createEdge('grasp-placement', 'gripper-above-ball', 'move-gripper-up', 1,
'placement')
graph.createEdge('ball-above-ground', 'grasp-placement', 'put-ball-down', 1,
'grasp')
graph.createEdge('grasp-placement', 'ball-above-ground', 'take-ball-up', 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,
])
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,
])