From a4c0a0d30e3ee14f80c3c9ad5021d6f20316ae6f Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 19 May 2022 17:11:25 +0200 Subject: [PATCH] add tests scripts --- tests/rtde_servojexample.py | 29 ++++++++++++++++++++++ tests/rtdetest1.py | 36 ++++++++++++++++++++++++++++ tests/servoj_circle.py | 48 +++++++++++++++++++++++++++++++++++++ tests/socket1.py | 38 +++++++++++++++++++++++++++++ 4 files changed, 151 insertions(+) create mode 100644 tests/rtde_servojexample.py create mode 100644 tests/rtdetest1.py create mode 100644 tests/servoj_circle.py create mode 100644 tests/socket1.py diff --git a/tests/rtde_servojexample.py b/tests/rtde_servojexample.py new file mode 100644 index 0000000..f255d3e --- /dev/null +++ b/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() \ No newline at end of file diff --git a/tests/rtdetest1.py b/tests/rtdetest1.py new file mode 100644 index 0000000..c18da1e --- /dev/null +++ b/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) diff --git a/tests/servoj_circle.py b/tests/servoj_circle.py new file mode 100644 index 0000000..80d6b62 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/tests/socket1.py b/tests/socket1.py new file mode 100644 index 0000000..a1f34a8 --- /dev/null +++ b/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 () \ No newline at end of file