# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/index.html
# Note: It is not required to keep a copy of this file, your python script is saved with the station
from robolink import *    # RoboDK API
from robodk import *      # Robot toolbox
RDK = Robolink()

#------ CONSTANT ------
ROBOT_NAME = 'TM12'
FRAME_NAME = 'LandMark_Base'


#------- Program ------
#robot = RDK.Item(ROBOT_NAME, ITEM_TYPE_ROBOT)
#frame = RDK.Item(FRAME_NAME, ITEM_TYPE_FRAME)

RDK.setSelection([])
robot = RDK.ItemUserPick('Robot',ITEM_TYPE_ROBOT)
frame = RDK.ItemUserPick('Frame',ITEM_TYPE_FRAME)

new_pose = eye()

for target in frame.Childs():
    robot.setPoseFrame(frame)
    print(target.Name())
    if target.Type() == ITEM_TYPE_TARGET:
        print(target.Pose()[:3,:3])
        print(robot.Pose()[:3,:3])
        print(new_pose)
        new_pose[0,3] = target.Pose()[0,3]
        new_pose[1,3] = target.Pose()[1,3]
        new_pose[2,3] = target.Pose()[2,3]
        new_pose[:3,:3] = robot.Pose()[:3,:3]
        target.setPose(new_pose)
        print(new_pose)