gaws_pendant.py

from move_group_python_interface_tutorial import *
from gcode_parser import GcodeParser
import re
import keyboard
import time
from gaws_robot_control import *
import sys
import os

class GAWSPendant():
    def __init__(self, robot):
        self.robot = robot
        self.keyboard_delay = 0.01
        self.frame = [0.0, 0.0, 0.8, 0.0, 0.0, 0.0]
        self.movement_resolution = 0.1
        self.cantgo = False

    def run(self):
        os.system("rm saved_robot_path.txt") # delete previous paths

        print("GAWS Pendant is running.")
        print("Tutorial:")
        print("key 'e' : +x")
        print("key 'q' : -x")
        print("key 'w' : +z")
        print("key 's' : -z")
        print("key 'a' : +y")
        print("key 'd' : -y")
        print("------------")
        print("key 'c' : cartesian save")
        print("key 'x' : fly save")

        frame = self.frame
        while True:
            time.sleep(0.01)

            # # Move along X coordinate system
            if keyboard.is_pressed('e'):
                frame[0] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 0
                coordinate_increased = True

            elif keyboard.is_pressed('q'):
                frame[0] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 0
                coordinate_increased = False

            # Move along Y coordinate system
            elif keyboard.is_pressed('d'):
                frame[1] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 1
                coordinate_increased = True

            elif keyboard.is_pressed('a'):
                frame[1] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 1
                coordinate_increased = False

            # Move along Z coordinate system
            elif keyboard.is_pressed('w'):
                frame[2] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 2
                coordinate_increased = True

            elif keyboard.is_pressed('s'):
                frame[2] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 2
                coordinate_increased = False

            # orientation X
            elif keyboard.is_pressed('r'):
                frame[3] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 3
                coordinate_increased = True

            elif keyboard.is_pressed('t'):
                frame[3] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 3
                coordinate_increased = False

            # orientation Y
            elif keyboard.is_pressed('f'):
                frame[4] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 4
                coordinate_increased = True

            elif keyboard.is_pressed('g'):
                frame[4] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 4
                coordinate_increased = False

            # orientation Z
            elif keyboard.is_pressed('v'):
                frame[5] += self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 5
                coordinate_increased = True
                
            elif keyboard.is_pressed('b'):
                frame[5] -= self.movement_resolution
                time.sleep( self.keyboard_delay )
                changed_index = 5
                coordinate_increased = False

            # Get CARTESIAN path and add it to the log file
            elif keyboard.is_pressed('c'):
                current_tcp_coordinates = str(self.robot.move_group.get_current_pose().pose).replace(" ", "").replace("position:", "").replace("orientation:", "").replace("x:", "").replace("y:", "").replace("z:", "").replace("w:", "").splitlines()
                coordinate_to_save = [
                    float(current_tcp_coordinates[1]),
                    float(current_tcp_coordinates[2]),
                    float(current_tcp_coordinates[3]),
                    float(current_tcp_coordinates[5]),
                    float(current_tcp_coordinates[6]),
                    float(current_tcp_coordinates[7])
                ]
                print("[INFO]: TCP at: ", coordinate_to_save)
                time.sleep(1)
                with open("saved_robot_path.txt", "a") as f:
                    f.write("fly(robot, " + str(coordinate_to_save) + ")\n")

                continue # MENDATORY (coordinate usually increases and program waits for that.)

            elif keyboard.is_pressed('x'):
                current_tcp_coordinates = str(self.robot.move_group.get_current_pose().pose).replace(" ", "").replace("position:", "").replace("orientation:", "").replace("x:", "").replace("y:", "").replace("z:", "").replace("w:", "").splitlines()
                coordinate_to_save = [
                    float(current_tcp_coordinates[1]),
                    float(current_tcp_coordinates[2]),
                    float(current_tcp_coordinates[3]),
                    float(current_tcp_coordinates[5]),
                    float(current_tcp_coordinates[6]),
                    float(current_tcp_coordinates[7])
                ]
                print("[INFO]: TCP at: ", coordinate_to_save)
                time.sleep(1)
                with open("saved_robot_path.txt", "a") as f:
                    f.write("cartesian_go(robot, " + str(coordinate_to_save) + ")\n")

                continue # MENDATORY (coordinate usually increases and program waits for that.)

            # Exit on ESC
            elif keyboard.is_pressed('esc'):
                os.system("clear")
                print("GAWS Pendant Finished.")
                sys.exit(0)
            else:
                continue

            # actual motion planning
            joints_before_plan = self.robot.move_group.get_current_joint_values()
            cartesian_plan, fraction = self.robot.go_cartesian(self.frame)
            robot.execute_plan(cartesian_plan)
            joints_after_plan = self.robot.move_group.get_current_joint_values()

            if joints_after_plan == joints_before_plan:
                if coordinate_increased:
                    frame[changed_index] -= self.movement_resolution
                elif not coordinate_increased:
                    frame[changed_index] += self.movement_resolution
                else:
                    print("[ERROR]: wtf?")

robot = MoveGroupPythonInterfaceTutorial()
pendant = GAWSPendant(robot)
pendant.run()