from robolink import *    # API de RoboDK
from robodk import *      # Toolbox pour la robotique
from threading import Event #import pour la pause entre chaque étape

# Démarrer la API de RoboDK:
RDK = Robolink()

# Prendre le robot (premier robot trouvé):
robot = RDK.Item('', ITEM_TYPE_ROBOT)
robot.setSpeed(speed_linear=30, speed_joints= 20, accel_linear=- 1, accel_joints=- 1)
gripO = RDK.Item('GripperOpen')
gripC = RDK.Item('GripperClose')

# Recup targets:
Home = RDK.Item('Home_gruyere')
olive_over = RDK.Item('olive_over')
olive = RDK.Item('olive')
gruyere_init = RDK.Item('gruyere_init')
gruyere_over_init = RDK.Item('gruyere_over_init')
chat_start = RDK.Item('chat_start')
chat_before_start = RDK.Item('chat_before_start')
chat_end = RDK.Item('chat_end')
before_grab = RDK.Item('before_grab')
grab = RDK.Item('grab')
grab_remove = RDK.Item('grab_remove')
inter_chat_grab = RDK.Item('intermediaire_chat_grab')
inter_home_chat = RDK.Item('intermediaire_home_chat')


pause=2
target_pose = gruyere_init.Pose()
xyz_ref = target_pose.Pos()

target_pose_over = gruyere_over_init.Pose()
xyz_ref_over = target_pose_over.Pos()

target_pose_actual = gruyere_init.Pose()
target_pose_actual_over = gruyere_over_init.Pose()

robot.setPoseFrame(robot.PoseFrame())

print (xyz_ref[0],xyz_ref[1],xyz_ref[2])
print (target_pose[0],"+",target_pose[1],target_pose[2],target_pose[3])
# Bouger le robot à la cible:
for x in range (0,1):
    for y in range(0,1):
        
        
        target_pose_actual.setPos([xyz_ref[0]-(30*x),xyz_ref[1]+(20*y),xyz_ref[2]])
        target_pose_actual_over.setPos([xyz_ref[0]-(30*x),xyz_ref[1]+(20*y),xyz_ref_over[2]])
        print('x =',x,'y=',y)
        print (xyz_ref[0]-(20*x),xyz_ref[1]-(30*y),xyz_ref[2])
        print(target_pose_actual)
        print(target_pose_actual_over)
        
        #RDK.RunMessage('début du programme', message_is_comment=False)
        RDK.RunProgram("GripperOpen")
        RDK.RunProgram("GripperClose")