48 lines
1.1 KiB
Python
48 lines
1.1 KiB
Python
|
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()
|