import rtde_control
import time
import math

rtde_c = rtde_control.RTDEControlInterface("192.168.2.164")

# 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


duration=20

xcenter=-0.5
ycenter=0.1
zcenter=0.5
c=0

cx=0

scale=0.1

rtde_c.moveJ([3.091137462206234, -1.2753459082058711, -1.7658837933568385, -1.6701929639036406, 1.5738425257277626, 4.658345567448585],0.1,0.1)

joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*0.2,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0])

print("Start q")
print(joint_q)



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 * 8
    c%= (2*3.1415)

    cx=i/(frequency*duration) * 2 *3.1415 * 1
    cx%= (2*3.1415)
    
    joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*scale,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()