from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations

RDK = Robolink()
RDK.setCollisionActive(COLLISION_ON)
RDK.setSimulationSpeed(1)

robot = RDK.Item('', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception("Robot not valid or not available")

robot.MoveJ(robot.JointsHome())

theta = pi/2
x = 70.7 + 200
y_range = [70.7, -70.7] 
z = 0
phi_range = [pi/2 + pi/4, pi + pi/4]
for i in range(0, 10):
    for j in range(0, 2):
        for y, phi in zip(y_range, phi_range):
            pose = transl(x,y,z)*rotx(pi - theta)*roty(phi - pi/2)*rotz(0)
            joints = robot.SolveIK(pose)
            robot.MoveJ(joints)
    robot.MoveJ(robot.JointsHome())