# 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

import numpy as np # for math calculation
from robolink import * # RoboDK API
from robodk import * # Robot toolbox

def Offset_pose(robot:robolink.Item,dist):
    target_ref = robot.Pose()
    target_i = Mat(target_ref)
    pos_i = target_i.Pos()
    pos_i[2]=pos_i[2]+dist
    target_i.setPos(pos_i)
    print('pose voulue')
    print(target_i)
    robot.MoveJ(target_i)

#runMode to RoboDK
runMode = 2 ##1=RUNMODE_SIMULATE/2=RUNMODE_RUN_ROBOT

#Connection to RoboDK
RDK = Robolink()

#Select Robot
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception('No robot selected or available')

ref = RDK.ItemUserPick('Select a robot', ITEM_TYPE_FRAME)
if not ref.Valid():
    raise Exception('No robot selected or available')

#Set RunMode
if runMode==1 :
    RDK.setRunMode(RUNMODE_SIMULATE)
elif runMode==2 :
    RDK.setRunMode(RUNMODE_RUN_ROBOT)

#Select Tool
tool = RDK.ItemUserPick('Select a tool', ITEM_TYPE_TOOL)
if not tool.Valid():
    raise Exception('No tool selected or available')

robot.setTool(tool)
robot.setPoseFrame(ref)
robot.setPoseTool(tool)

# go to start position
ShowMessage("go to starting position")
robot.setSpeed(30,30,1,1)
robot.MoveJ([5.28, -28.72, 79.18,0.00, 39.54, -174.72])

print("Move up and down 50mm")
print(robot.Pose())
Offset_pose(robot,50)
print('Pose réelle')
print(robot.Pose())
Offset_pose(robot,-50)
print('Pose réelle')
print(robot.Pose())