# -*- coding: utf-8 -*-
# UFG-EMC Introdução a Robótica de Manipuladores
# 2021-05-05 Marco Antonio Assfalk de Oliveira
# ---
# Uma versão em python e robolink/dk APIs para os comandos IMOV
# da linguagem INFORM III (Motoman/Yaskawa).
# ---
# Supõe:
#  (a) a sequencia IMOVE seja definida como targets IPOS* dentro da simulação,
# inicialmente definidos com relação a posição REF;
#  (b) um robo 'Motoman HP20D';
#  (c) uma posição 'Home'.
# ---
# Apresenta uma alternativa sem visualização dos IMOV prefixado com "codigo (2)"
# ---

from robolink import * # RoboDK's API

from robodk import * # Math toolbox for robots


# Start the RoboDK API:

RDK = Robolink()


# Get the robot item by name:

robot = RDK.Item('Motoman HP20D', ITEM_TYPE_ROBOT)


# Get the targets by name:

Home = RDK.Item('Home')

# Obter da simulação os deslocamentos IMOV equivalentes,
#  definidos posições IPOS com relação ao ponto REF
iPos1 = RDK.Item('IPOS1')
iPos2 = RDK.Item('IPOS2')
iPos0 = RDK.Item('IPOS0')
iPos3 = RDK.Item('IPOS3')
Referencia = RDK.Item('REF1')

Ref_pose = Referencia.Pose()

xyz_refR=[0,0,0,0,0,0]
xyz_refR[0:2] = Ref_pose.Pos()
# extrair Rx,Ry,Rz
# Unable to find function, setting to [-180,0,0]
xyz_refR[3:5] = [-180,0,0]

# Extrair deslocamentos e rotações relativas
# 1. Extrair pose e transformar para xyz_ref
iPos0_pose = iPos0.Pose()
xyz_ref0=[0,0,0,0,0,0]
xyz_ref0[0:2] = iPos0_pose.Pos()
# extrair Rx,Ry,Rz
# xyz_ref0[3],xyz_ref0[4],xyz_ref0[5] = iPos0_pose.rotationPose()
# print(iPos0_pose.rotationPose())
# Unable to find function, setting to [-180,0,0]
xyz_ref0[3:5] = [-180,0,0]

dPos0=[0,0,0,0,0,0]
iPos1_pose = iPos1.Pose()
xyz_ref1=[0,0,0,0,0,0]
xyz_ref1[0:2] = iPos1_pose.Pos()
# extrair Rx,Ry,Rz
# xyz_ref1[3:5] = iPos1_pose.rotationPose()
# Unable to find function, setting to [-180,0,0]
xyz_ref1[3:5] = [-180,0,0]

dPos1=[0,0,0,0,0,0]
iPos2_pose = iPos2.Pose()
xyz_ref2=[0,0,0,0,0,0]
xyz_ref2[0:2] = iPos2_pose.Pos()
# extrair Rx,Ry,Rz
# xyz_ref2[3:5] = iPos2_pose.rotationPose()
# Unable to find function, setting to [-180,0,0]
xyz_ref2[3:5] = [-180,0,0]
dPos2=[0,0,0,0,0,0]

# 2. Calcular apenas as translações/rotações relativas
for index in range(0,5):
        #print(index, dPos0[index], xyz_ref0[index]);
	dPos0[index] = xyz_ref0[index] - xyz_refR[index]
	dPos1[index] = xyz_ref1[index] - xyz_refR[index]
	dPos2[index] = xyz_ref2[index] - xyz_refR[index]

# Move the robot to the Home point:

robot.MoveJ(Home)

# Move the robot to the Home point:

robot.MoveJ(Referencia)

# Criei um target temporário para visualizar a atualização do IMOVE no simulador
# Se não for desejado, simplesmente utilize o código (2) e comentar as instruções
# entre os comentários #(1) e #/(1)
iTarget= RDK.AddTarget('TempTarget')

# Executar a sequencia três vezes, incrementando a posição referencia em Z de 100 mm
# O deslocamento foi realizado diretamente, mas um programa mais claro utilizaria uma ou
# mais variáveis contendo o delta em/a redor de cada eixo dx, dy, dz, dQx, dQy, dQz
# dPos0+i*[dx dy dz dQx dQy dQz]

for i in range(3):

# Executar i-ésima sequencia IMOVE relativo a IREF1

#(1)
# IMOVE posição 1 (iPos0)
# extrair xyz_ref temporários e aplicar translação
	ix,iy,iz,iQx,iQy,iQz = dPos0
	iz=iz+i*100
# mudar posição/orientação temporária iTarget
# utilizando robodk.Offset
	testPos=Offset(Referencia.Pose(),ix,iy,iz,iQx,iQy,iQz)
	iTarget.setPose(testPos)
	robot.MoveL(iTarget)
# código (2) robot.MoveL(Offset(Ref1.Pos(), (dPos0+[0 0 i*100 0 0 0])))

# IMOVE posição 2 (iPos1)
# extrair xyz_ref temporários e aplicar translação
	ix,iy,iz,iQx,iQy,iQz = dPos1
	iz=iz+i*100
# mudar posição/orientação temporária iTarget
# utilizando robodk.Offset
	iTarget.setPose(Offset(Referencia.Pose(),ix,iy,iz,iQx,iQy,iQz))
	robot.MoveL(iTarget)
	
# IMOVE posição 3 (iPos2)
# extrair xyz_ref temporários e aplicar translação
	ix,iy,iz,iQx,iQy,iQz = dPos2
	iz=iz+i*100
# mudar posição/orientação temporária iTarget
# utilizando robodk.Offset
	iTarget.setPose(Offset(Referencia.Pose(),ix,iy,iz,iQx,iQy,iQz))
	robot.MoveL(iTarget)
#/(1)

# código (2) 
# 	robot.MoveL(Offset(Ref1.Pos(), (dPos0+[0 0 i*100 0 0 0])))
# 	robot.MoveL(Offset(Ref1.Pos(), (dPos1+[0 0 i*100 0 0 0])))
# 	robot.MoveL(Offset(Ref1.Pos(), (dPos2+[0 0 i*100 0 0 0])))

# voltar para Home

robot.MoveL(Home)

# Limpar TempTarget
iTarget.Delete()

print('Fim')