from robodk.robolink import *
from robodk.robomath import *

RDK = Robolink()

J1 = 0
J2 = 0
J3 =0
J4 = 0
J5 = 0
J6 = 0
e1 = 0

robot = RDK.Item('', ITEM_TYPE_ROBOT) # Get first robot in cell
robot.setJoints([J1,J2,J3,J4,J5,J6,e1]) # Set all joints and rail to 0

reference = RDK.Item('TableFrame', ITEM_TYPE_FRAME) # Get the reference frame for the table
print(reference)
if not reference.Valid():
	raise Exception("No reference frame found!")

# referencePose = reference.Pose() # Get the pose of the reference frame
# print(referencePose)

toolFrame = RDK.Item('TapeHead', ITEM_TYPE_TOOL) # get the tool frame for the tape head
if not toolFrame.Valid():
	raise Exception('No tool frame found!')

target1 = RDK.Item('Target 1')

print(target1.Joints())
target1.setAsCartesianTarget()
target1.Joints()

print(target1.Joints())
robot.setPoseFrame(reference)
robot.setPoseTool(toolFrame)

robot.MoveJ(target1)
print("Current robot joint values: \n"+ str(robot.Joints().list()))
print("Joint Values for Target 1: \n"+ str(target1.Joints().list()))