from robodk.robolink import Robolink, ITEM_TYPE_ROBOT, ITEM_TYPE_TARGET, ITEM_TYPE_FRAME, ITEM_TYPE_TOOL
from robodk.robomath import eye
from robodk import robomath
import numpy as np
import time
RDK = Robolink()
pose = eye()

RDK.setRunMode(run_mode=6)

# Get the robot, frame and tool objects
robot = RDK.Item('Dobot Magician')

p1 = robomath.Mat([
    [1., 0., 0., 250.0],
    [0., 1., 0.,    0.0],
    [0., 0., 1., 100.0],
    [0., 0., 0.,    1.0],
])
p2 = robomath.Mat([
    [1., 0., 0., 280.0],
    [0., 1., 0.,    0.0],
    [0., 0., 1., 100.0],
    [0., 0., 0.,    1.0],
])

robot.MoveJ([0, 0, 0, 0,])
robot.MoveJ([0, 20, 30, 0,])

robot.MoveL(p1)
robot.MoveL(p2)
robot.MoveJ([0, 0, 0, 0,])

print('Done')