From 1e50f1f0a51147ab8da6d11e4f5094d44bd874f4 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Thu, 9 May 2024 13:06:44 +0200
Subject: [PATCH] move file

---
 curses_actualq.py | 153 ++++++++++++++++++++++++++++++++++++++++++++++
 requirements.txt  |   1 +
 2 files changed, 154 insertions(+)
 create mode 100644 curses_actualq.py

diff --git a/curses_actualq.py b/curses_actualq.py
new file mode 100644
index 0000000..ee5422b
--- /dev/null
+++ b/curses_actualq.py
@@ -0,0 +1,153 @@
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import time
+import argparse
+import sys
+import curses
+import math
+
+
+def toRad(d):
+    return d/360*2*math.pi
+
+def toDeg(r):
+    return r*360/2/math.pi
+
+
+def mapFromTo(x,a,b,c,d):
+   y=(x-a)/(b-a)*(d-c)+c
+   return y
+
+
+def parse_args(args):
+    """Parse command line parameters
+
+    Args:
+      args ([str]): command line parameters as list of strings
+
+    Returns:
+      :obj:`argparse.Namespace`: command line parameters namespace
+    """
+    parser = argparse.ArgumentParser(
+        description="Record data example")
+    parser.add_argument(
+        "-ip",
+        "--robot_ip",
+        dest="ip",
+        help="IP address of the UR robot",
+        type=str,
+        default='localhost',
+        metavar="<IP address of the UR robot>")
+    parser.add_argument(
+        "-o",
+        "--output",
+        dest="output",
+        help="data output (.csv) file to write to (default is \"robot_data.csv\"",
+        type=str,
+        default="robot_data.csv",
+        metavar="<data output file>")
+    parser.add_argument(
+        "-f",
+        "--frequency",
+        dest="frequency",
+        help="the frequency at which the data is recorded (default is 500Hz)",
+        type=float,
+        default=500.0,
+        metavar="<frequency>")
+
+    return parser.parse_args(args)
+
+joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)]
+joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)]
+
+
+
+def main(stdscr):
+    args=sys.argv[1:]
+    """Main entry point allowing external calls
+
+    Args:
+      args ([str]): command line parameter list
+    """
+
+    # Clear and refresh the screen for a blank canvas
+    stdscr.clear()
+    stdscr.refresh()
+
+    curses.cbreak()
+    stdscr.keypad(1)
+    stdscr.nodelay(1)
+
+    
+    # Start colors in curses
+    curses.start_color()
+    curses.init_pair(1, curses.COLOR_CYAN, curses.COLOR_BLACK)
+    curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK)
+    curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_WHITE)
+
+    
+    height, width = stdscr.getmaxyx()
+    
+
+    args = parse_args(args)
+    dt = 1 / args.frequency
+    rtde_r = RTDEReceive(args.ip, args.frequency)
+    
+    
+    key = ''
+    i = 0
+    try:
+        while key != ord('q'):
+            key = stdscr.getch()
+            stdscr.clear()
+            start = time.time()
+            
+            q=rtde_r.getActualQ()
+            #print(q)
+            #sys.stdout.write("\r")
+            #sys.stdout.write("{:3d} samples.".format(i))
+            #sys.stdout.flush()
+
+            stdscr.addstr(0, 0, "test "+str(i), curses.color_pair(1))
+
+
+            for j in range(6):
+                posx=mapFromTo(q[j],joint_min[j],joint_max[j],0,width) #scale to screen width
+                for px in range(width):
+                    diff=abs(posx-px)
+                    poschar=''
+                    if diff<3:
+                        poschar='.'
+                    if diff<2:
+                        poschar='i'
+                    if diff<0.5:
+                        poschar='|'
+                        
+                    if (len(poschar)>0):
+                        stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
+                valuetext=str(round(q[j],2))
+                if q[j]>=0: #has no negative sign
+                    valuetext=" "+valuetext
+                stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))
+
+                
+            # Refresh the screen
+            stdscr.refresh()
+
+
+            end = time.time()
+            duration = end - start
+
+            if duration < dt:
+                time.sleep(dt - duration)
+            i += 1
+
+    except KeyboardInterrupt:
+        
+        print("\nData recording stopped.")
+        curses.endwin()
+        exit()
+
+
+if __name__ == "__main__":
+    curses.wrapper(main)
+    
\ No newline at end of file
diff --git a/requirements.txt b/requirements.txt
index 42c7ca6..ec5b0a8 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,2 +1,3 @@
 ur_rtde
 numpy
+windows-curses
\ No newline at end of file