29 lines
724 B
Python
29 lines
724 B
Python
|
import rtde_control
|
||
|
|
import time
|
||
|
|
|
||
|
|
rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
|
||
|
|
|
||
|
|
# Parameters
|
||
|
|
velocity = 0.1 #0.5
|
||
|
|
acceleration = 0.1 #0.5
|
||
|
|
dt = 1.0/500 # 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(joint_q, 0.1, 0.1)
|
||
|
|
|
||
|
|
# Execute 500Hz control loop for 2 seconds, each cycle is 2ms
|
||
|
|
for i in range(1000):
|
||
|
|
start = time.time()
|
||
|
|
rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
|
||
|
|
joint_q[0] += 0.001
|
||
|
|
joint_q[1] += 0.001
|
||
|
|
end = time.time()
|
||
|
|
duration = end - start
|
||
|
|
if duration < dt:
|
||
|
|
time.sleep(dt - duration)
|
||
|
|
|
||
|
|
rtde_c.servoStop()
|
||
|
|
rtde_c.stopScript()
|