#! /usr/bin/python3
import time
import socket
import math
import rtde_control

import rtde_receive
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101")



actual_q = rtde_r.getActualQ()
print(actual_q)

print("faehrt")


ycenter=0.2
zcenter=0.5
c=0

scale=0.2

rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
#rtde_c.moveJ([-0.10, -0.5, 0.20, -0.001, 3.12, 0.04], 0.1, 0.1)
#rtde_c.moveJ([-1.54, -1.83, -2.28, -0.59, 1.60, 0.023], 0.1, 0.1)
rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1)
rtde_c.moveJ(rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]), 0.1,0.1)

print("gefahren")

rtde_c.servoStop()
rtde_c.stopScript()

actual_q = rtde_r.getActualQ()
print(actual_q)