sim2real-robot-identification/console.py at main · iit-DLSLab/sim2real-robot-identification · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
import readline
import readchar
import time

import numpy as np
import copy

import mujoco

class Console():
    def __init__(self, controller_node):
        self.controller_node = controller_node

        self.isDown = True
        self.setpoint_collection = False
        self.falling_collection = False
        self.trajectory_collection = False
        self.isActivated = False

        # Autocomplete setup
        self.commands = [
            "help", "goDown", "startGeneration", "setKp", "setKd"
        ]
        readline.set_completer(self.complete)
        readline.parse_and_bind("tab: complete")


    def complete(self, text, state):
        options = [cmd for cmd in self.commands if cmd.startswith(text)]
        if state < len(options):
            print(options[state])
            return options[state]
        else:
            return None


    def interactive_command_line(self, ):
        self.print_all_commands()
        while True:
            input_string = input(">>> ")
            try:

                if(input_string == "startCollection"):

                    mode = input("Select the collection mode (setpoint/falling/trajectory): ")
                    if(mode == "setpoint"):
                        self.setpoint_collection = True
                        self.falling_collection = False
                        self.trajectory_collection = False
                        print("Setpoint collection mode activated")
                    elif(mode == "falling"):
                        self.setpoint_collection = False
                        self.falling_collection = True
                        self.trajectory_collection = False
                        print("Falling collection mode activated")
                    elif(mode == "trajectory"):
                        self.setpoint_collection = False
                        self.falling_collection = False
                        self.trajectory_collection = True
                        print("Trajectory collection mode activated")
                    else:
                        print("Invalid mode selected")
                        self.isActivated = False
                        continue

                    self.isActivated = True

                elif(input_string == "help"):
                    self.print_all_commands()


                elif(input_string == "setKp"):
                    print("Kp stand_up_and_down: ", self.controller_node.Kp_stand_up_and_down)
                    temp = input("Enter Kp: ")
                    if(temp != ""):
                        self.controller_node.Kp_stand_up_and_down = float(temp)


                elif(input_string == "setKd"):
                    print("Kd stand_up_and_down: ", self.controller_node.Kd_stand_up_and_down)
                    temp = input("Enter Kd: ")
                    if(temp != ""):
                        self.controller_node.Kd_stand_up_and_down = float(temp)


            except Exception as e:
                print("Error: ", e)
                print("Invalid Command")
                self.print_all_commands()


    def print_all_commands(self):
        print("\nAvailable Commands")
        print("help: Display all available messages")
        print("goDown: Move the robot down")
        print("startCollection: Start the collection process")
        print("setKp: Set the proportional gain")
        print("setKd: Set the derivative gain\n")