픽 앤 플레이스 실현

PROC Sample_9()
    !set the acceleration parameters
    AccSet 50, 50;
    !set the velocity parameters
    VelSet 50, 1000;
    !move to robot home position
    MoveAbsJ home\NoEOffs,v500,fine,gripper1;
    !initialize communication parameters (initialization is required only once)
    MM_Init_Socket "192.168.10.111",50000,300;
    !move to image-capturing position
    MoveL camera_capture,v1000,fine,gripper1;
    !open socket connection
    MM_Open_Socket;
    !trigger Mech-Viz project
    MM_Start_Viz 2,snap_jps;
    !set branch exit port
    MM_Set_Branch 1,1;
LOOP:
    !get planned path, 1st argument (1) means getting pose in JPs
    MM_Get_VizData 1, pose_num, vis_pose_num, status;
    !check whether planned path has been got from Mech-Viz successfully
    IF status <> 2100 THEN
        !add error handling logic here according to different error codes
        !e.g.: status=2038 means no point cloud in ROI
        Stop;
    ENDIF
    !save waypoints of the planned path to local variables one by one
    MM_Get_JPS 1,jps{1},label{1},speed{1};
    MM_Get_JPS 2,jps{2},label{2},speed{2};
    MM_Get_JPS 3,jps{3},label{3},speed{3};
    !follow the planned path to pick
    !move to approach waypoint of picking
    MoveAbsJ jps{1},v500,fine,gripper1;
    !move to picking waypoint
    MoveAbsJ jps{2},v300,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;"
    setDo do0, 1;
    !Stop;
    !trigger Mech-Viz project but not to trigger camera capturing
    MM_Start_Viz 2,snap_jps;
    !move to departure waypoint of picking
    MoveAbsJ jps{3},v500,fine,gripper1;
    !move to intermediate waypoint of placing
    MoveJ drop_waypoint,v500,z50,gripper1;
    !move to approach waypoint of placing
    MoveL RelTool(drop,0,0,-100),v500,fine,gripper1;
    !set branch exit port and trigger camera capturing when robot moves out of camera’s field of view
    MM_Set_Branch 1,1;
    !move to placing waypoint
    MoveL drop,v300,fine,gripper1;
    !add object releasing logic here, such as "setdo DO_1, 0;"
    !Stop;
    setDo do0, 0;
    setDo do4, 1;
    !move to departure waypoint of placing
    MoveL RelTool(drop,0,0,-100),v500,fine,gripper1;
    setDo do4, 0;
    !move back to robot home position
    MoveAbsJ home\NoEOffs,v500,fine,gripper1;
    GOTO LOOP;
END_LOOP:
    !close socket connection
    MM_Close_Socket;
ENDPROC
ENDMODULE