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, ])