from robolink import *    # API to communicate with RoboDK
from robodk import *      # robodk robotics toolbox
import numpy as np


# Any interaction with RoboDK must be done through RDK:
RDK = Robolink()

# get the robot by name:
robot = RDK.Item('', ITEM_TYPE_ROBOT)

# try the poses
for i in range(0, 363, 2):
    pose = np.loadtxt("/home/robi/PycharmProjects/burr_scanning/icp_transforming_smoothing/poses_t/pose_t_" + str(i) + ".txt")
    # print(pose)
    posearr = robodk.Pose_2_TxyzRxyz(pose)
    # posearr = robodk.Pose_2_ABB(pose)
    print(posearr)

    #add new target
    target_number = i
    frame = RDK.Item('Frame ' + str(i))
    # frame = RDK.Item('Frame 4')
    Target = RDK.AddTarget('Scan ' + str(target_number), frame)
    Target.setPose(Offset(eye(), posearr[0], posearr[1], posearr[2], posearr[3]+180, posearr[4], posearr[5]))

for i in range(0,360,2):
    target = RDK.Item('Scan ' + str(i))    
    robot.MoveJ(target)