153 lines
No EOL
3.8 KiB
Python
153 lines
No EOL
3.8 KiB
Python
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)
|
|
|