import rtde_control
import time
import math

rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")

# Parameters
velocity = 0.1   #0.5
acceleration = 0.1   #0.5
frequency= 100
dt = 1.0/frequency  # 2ms
lookahead_time = 0.1
gain = 300
joint_q = [-1.54, -1.83, -2.28, -0.59, 1.60, 0.023]

# Move to initial joint position with a regular moveJ
rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1)

duration=10

ycenter=0.2
zcenter=0.5
c=0

scale=0.2

print("Starting Servo")
# Execute 500Hz control loop for 2 seconds, each cycle is 2ms
for i in range(frequency*duration):
    start = time.time()
    
    c=i/(frequency*duration) * 2 *3.1415 * 4
    c%= (2*3.1415)
    
    joint_q=rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0])
    #print(joint_q)
    rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
    
    end = time.time()
    stepduration = end - start
    
    if stepduration < dt:
        time.sleep(dt - stepduration)

print("Finished")

rtde_c.servoStop()
rtde_c.stopScript()