add tests scripts

tests/rtde_servojexample.py

@@ -0,0 +1,29 @@
+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()

tests/rtdetest1.py

@@ -0,0 +1,36 @@
+#! /usr/bin/python3
+import time
+import socket
+import math
+import rtde_control
+
+import rtde_receive
+rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101")
+
+
+
+actual_q = rtde_r.getActualQ()
+print(actual_q)
+
+print("faehrt")
+
+
+ycenter=0.2
+zcenter=0.5
+c=0
+
+scale=0.2
+
+rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
+#rtde_c.moveJ([-0.10, -0.5, 0.20, -0.001, 3.12, 0.04], 0.1, 0.1)
+#rtde_c.moveJ([-1.54, -1.83, -2.28, -0.59, 1.60, 0.023], 0.1, 0.1)
+rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1)
+rtde_c.moveJ(rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]), 0.1,0.1)
+
+print("gefahren")
+
+rtde_c.servoStop()
+rtde_c.stopScript()
+
+actual_q = rtde_r.getActualQ()
+print(actual_q)

tests/servoj_circle.py

@@ -0,0 +1,48 @@
+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()

tests/socket1.py

@@ -0,0 +1,38 @@
+#! /usr/bin/python3
+import time
+import socket
+import math
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+host ="192.168.1.101"
+port =30002
+s.connect((host,port))
+
+
+c=0
+ycenter=0.2
+zcenter=0.5
+
+scale=0.2
+
+frequency=20
+lookahead_time= 0.05  # range [0.03,0.2]
+servo_t = 1.0/frequency*2 + lookahead_time
+gain = 100 #range [100,2000]
+
+last_send = 0
+while True:
+
+    if time.time()>=last_send + (1.0/frequency):
+        last_send=time.time()
+        #text="movel(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=1.2, v=0.1, t=0.2, r=0.02)\n"
+        #text="movep(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=0.5, v=0.1, r=0.05)\n"
+        text="servoj(get_inverse_kin(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0]), t="+str(servo_t)+", lookahead_time="+str(lookahead_time)+", gain="+str(gain)+")\n"
+        s.send(text.encode())
+        print(text)
+        
+        #time.sleep(1.0/frequency)
+        c+=0.1
+        c%=2*3.1415
+
+s.close ()