63 lines
1.4 KiB
Python
63 lines
1.4 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
|
|
|
|
|
|
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)*0.2,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()
|