def __init__(self): # Initialize the driver. super(Driver, self).__init__() # Instantiate emitter node to send data to receivers. self.emitter = self.getDevice('emitter_supervisor') # Instantiate receiver node to obtain data from emitters. self.receiver = self.getDevice('receiver_supervisor') self.receiver.enable(self.TIME_STEP) # Create list for emitted message. # ========== AGV ROBOTS ========== # Instantiate the AGV robots. self.AGV_ROBOT_1 = self.getFromDef('AGV_ROBOT1') self.robot_status = False # Instantiate suction gripper self.SUCTION_CUP = self.getFromDef('SUCTION_CUP') self.MAX_NUM_PALLETS = 13 self.AGV_PALLET_NO = (self.MAX_NUM_PALLETS - 1) self.create_pallets(num_pallets=self.MAX_NUM_PALLETS) self.picked_packets = [] # Set initial GPS values for storing of updated packet positions. self.current_gps_pos = [0, 0, 0] self.previous_gps_pos = [0, 0, 0] # Enable the keyboard. self.kb = Keyboard() self.kb.enable(self.TIME_STEP) self.run()
def start_simulation(self): robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) motor = robot.getMotor('propeller') visual = robot.getMotor('visual') sensor = robot.getPositionSensor('sensor') sensor.enable(TIME_STEP) motor.setVelocity(100) motor.setPosition(self.initial_position) visual.setVelocity(0) visual.setPosition(float('inf')) visual.setTorque(100) while robot.step(TIME_STEP) != -1: if not self.initial_position_reached(sensor): continue else: motor.setVelocity(0) motor.setPosition(float('inf')) self.current_angle = sensor.getValue() motor.setTorque(self.motor_velocity * self.length + self.disturbance * (random.random() - 0.5)) visual.setVelocity(150 * self.motor_velocity)
def start_simulation(self): self.total_energy = 0 robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) #camera = robot.getCamera('view') #camera.enable(TIME_STEP) motor = robot.getMotor('motor') sun_motor = robot.getMotor('sun_motor') panel_sensor = robot.getPositionSensor('panel_sensor') sun_sensor = robot.getPositionSensor('sun_sensor') panel_sensor.enable(TIME_STEP) sun_sensor.enable(TIME_STEP) if self.moving_sun: sun_motor.setVelocity(self.sun_velocity) sun_motor.setPosition(self.max_sun_position) else: sun_motor.setVelocity(10) random_pos = random.random() * math.pi - math.pi / 2 sun_motor.setPosition(0) while (abs(sun_sensor.getValue() - random_pos) > 1e-2): time.sleep(0.1) printed = False t = 0 i = 0 while robot.step(TIME_STEP) != -1: sun_position = sun_sensor.getValue() panel_position = panel_sensor.getValue() self.sun_position = sun_position self.panel_position = panel_position if list(self.controller_output): self.set_torque(self.controller_output[i]) if (self.moving_sun and abs(sun_position - self.max_sun_position) < 1e-1): if not printed: print(f"Energy harvested: {self.total_energy} J") printed = True continue motor.setTorque(self.motor_torque + self.disturbance * (random.random() - 0.5)) self.total_energy += max( [0, 1 - abs(panel_position - sun_position)]) * TIME_STEP t += TIME_STEP * 1e-3 i += 1
def start_simulation(self): robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) motor = robot.getMotor('propeller') visual = robot.getMotor('visual') motor.setVelocity(0) motor.setPosition(float('inf')) visual.setVelocity(0) visual.setPosition(float('inf')) visual.setTorque(100) while robot.step(TIME_STEP) != -1: motor.setTorque(-self.motor_velocity * self.length) visual.setVelocity(150 * self.motor_velocity)
def handle_gui(self): if not self.keyboard_activated: self.keyboard = Keyboard() self.keyboard.enable(100) self.keyboard_activated = True key = self.keyboard.getKey() if key == ord('R'): self.reset() elif key == ord('P'): self.set_initial_poses() elif key == Keyboard.SHIFT + ord('R'): try: self.reset_ball() except AttributeError: print("No ball in simulation that can be reset") return key
def __init__(self): # Initialize Robot self.robot = Robot() self.time_step = int(self.robot.getBasicTimeStep()) print("Robot time step ", self.time_step) # Initialize sensors self.metacamera = self.robot.getCamera("MultiSense S21 meta camera") self.leftcamera = self.robot.getCamera("MultiSense S21 left camera") self.rightcamera = self.robot.getCamera("MultiSense S21 right camera") self.rangeFinder = self.robot.getRangeFinder( "MultiSense S21 meta range finder") self.cameras = { "left": self.leftcamera, "right": self.rightcamera, "meta": self.metacamera, "depth": self.rangeFinder } self.leftpositionsensor = self.robot.getPositionSensor( 'back left wheel sensor') self.rightpositionsensor = self.robot.getPositionSensor( 'back right wheel sensor') self.keyboard = Keyboard() self.pen = self.robot.getPen('pen') # Enable sensors self.metacamera.enable(self.time_step) self.leftcamera.enable(self.time_step) self.rightcamera.enable(self.time_step) self.rangeFinder.enable(self.time_step) self.leftpositionsensor.enable(self.time_step) self.rightpositionsensor.enable(self.time_step) self.keyboard.enable(self.time_step) # Initialize wheels backrightwheel = self.robot.getMotor('back right wheel') backleftwheel = self.robot.getMotor('back left wheel') frontrightwheel = self.robot.getMotor('front right wheel') frontleftwheel = self.robot.getMotor('front left wheel') self.wheels = [ backrightwheel, backleftwheel, frontrightwheel, frontleftwheel ] self.set_velocity_control() self.prevlspeed = 0 self.prevrspeed = 0 self.curlspeed = 0 self.currspeed = 0
def start_simulation(self): self.total_energy = 0 robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) camera = robot.getCamera('view') camera.enable(TIME_STEP) motor = robot.getMotor('motor') sun_motor = robot.getMotor('sun_motor') panel_sensor = robot.getPositionSensor('panel_sensor') sun_sensor = robot.getPositionSensor('sun_sensor') panel_sensor.enable(TIME_STEP) sun_sensor.enable(TIME_STEP) if self.moving_sun: sun_motor.setVelocity(0.3) sun_motor.setPosition(self.max_sun_position) else: sun_motor.setVelocity(10) random_pos = random.random() * math.pi - math.pi / 2 sun_motor.setPosition(random_pos) while (abs(sun_sensor.getValue() - random_pos) > 1e-2): time.sleep(0.1) start_time = time.time() printed = False while robot.step(TIME_STEP) != -1: camera.getImage() sun_position = sun_sensor.getValue() panel_position = panel_sensor.getValue() if (self.moving_sun and abs(sun_position - self.max_sun_position) < 1e-1)\ or (not self.moving_sun and time.time() - start_time > self.time_limit): if not printed: print("Energy harvested:", self.total_energy, "J") printed = True continue motor.setTorque(self.motor_torque) self.total_energy += max( [0, 1 - abs(panel_position - sun_position)])
class KeyboardPrinter(SupervisorEnv): def __init__(self, controller): self.controller = controller self.keyboard = Keyboard() self.keyboard.enable(self.controller.get_timestep()) def step(self, action): observation, reward, isDone, info = self.controller.step(action) key = self.keyboard.getKey() # DEBUG CONTROLS if key == Keyboard.CONTROL + ord("A"): print() print("Actions: ", action) if key == Keyboard.CONTROL + ord("R"): print() print("Rewards: ", reward) if key == Keyboard.CONTROL + ord("Y"): print() print("Observations: ", self.controller.observation) return observation, reward, isDone, info def is_done(self): isDone = self.controller.is_done() if isDone: print("Done") return isDone def get_observations(self): return self.controller.get_observations() def get_reward(self, action): return self.controller.get_reward(action) def get_info(self): return self.controller.get_info() def reset(self): print("RESET") observations = self.controller.reset() return observations
class Driver(Supervisor): TIME_STEP = 32 x = int(0.0) y = int(0.0) z = int(0.0) translation = [x, y, z] # Instantiated objects AGV_ROBOT_1 = None SUCTION_CUP = None #agv_robot_1_translation_field = agv_robot_1.getField('translation') #suction_cup = self.getFromDef('suction_cup') #box_1 = # Variables for AGV, pallets and packets picking_started = False picking_finished = False robot_status = False MAX_NUM_PALLETS = None AGV_PALLET_NO = None count_now = False packet_removed = False packet_added = False # Pallets pallet_list = None pallet_num = None # Packets picked_packets = None packet_list = None packet_num = None agv_packet_num = None # Variables to hold and update the location of packets. current_gps_pos = None previous_gps_pos = None # ----------------- COMMUNICATION --------------- # Instantiate receiver node to obtain data from emitters. emitter = None receiver = None # Message sent by emitter. emitted_message = None previous_message = None # ------------------- KEYBOARD -------------- # # Enable the keyboard. # kb = Keyboard() # kb.enable(TIME_STEP) # Keyboard values for AGV FORWARD = '315' BACKWARD = '317' TURN_LEFT = '314' TURN_RIGHT = '316' SPEED_INCREASE_AGV = '43' SPEED_DECREASE_AGV = '45' # Keyboard values for Hasselhoff Hug - Axis 1 LEFT_AXIS_1 = '65' RIGHT_AXIS_1 = '68' SPEED_INCREASE_SNAKEBOX = str(Keyboard.CONTROL + Keyboard.SHIFT + 43) SPEED_DECREASE_SNAKEBOX = str(Keyboard.CONTROL + Keyboard.SHIFT + 45) # Keyboard values for Tower - Axis 2 UP_AXIS_2 = '87' DOWN_AXIS_2 = '83' SPEED_INCREASE_TOWER = str(Keyboard.CONTROL + 43) SPEED_DECREASE_TOWER = str(Keyboard.CONTROL + 45) # Keyboard values for Snake - Axis 3 EXTEND_AXIS_3 = '81' RETRACT_AXIS_3 = '69' SPEED_INCREASE_SNAKE = str(Keyboard.SHIFT + 43) SPEED_DECREASE_SNAKE = str(Keyboard.SHIFT + 45) # Keyboard values for Snake tip - Axis 4 LEFT_AXIS_4 = '90' RIGHT_AXIS_4 = '67' # Keyboard values for Packet handling PACKET_ATTACH = '32' PACKET_DETACH = '65568' def __init__(self): # Initialize the driver. super(Driver, self).__init__() # Instantiate emitter node to send data to receivers. self.emitter = self.getDevice('emitter_supervisor') # Instantiate receiver node to obtain data from emitters. self.receiver = self.getDevice('receiver_supervisor') self.receiver.enable(self.TIME_STEP) # Create list for emitted message. # ========== AGV ROBOTS ========== # Instantiate the AGV robots. self.AGV_ROBOT_1 = self.getFromDef('AGV_ROBOT1') self.robot_status = False # Instantiate suction gripper self.SUCTION_CUP = self.getFromDef('SUCTION_CUP') self.MAX_NUM_PALLETS = 13 self.AGV_PALLET_NO = (self.MAX_NUM_PALLETS - 1) self.create_pallets(num_pallets=self.MAX_NUM_PALLETS) self.picked_packets = [] # Set initial GPS values for storing of updated packet positions. self.current_gps_pos = [0, 0, 0] self.previous_gps_pos = [0, 0, 0] # Enable the keyboard. self.kb = Keyboard() self.kb.enable(self.TIME_STEP) self.run() # ========== PACKET ROBOTS ========== # Instantiate the active packets/boxes. # Continuously reads the keyboard for user inputs. # Reads up to 7 simultaneous/combined key presses. def read_keyboard(self): # ------ KEYBOARD ----- # https://cyberbotics.com/doc/reference/keyboard # Register keystrokes keystrokes = [str(-1)] * 7 for i in range(0, 7): keystrokes[i] = str(self.kb.getKey()) # print(keystrokes) return keystrokes def error_testing_thingy(self, num): assert isinstance(num, int), INVALID_NUM_MSG def instantiate_packets(self): try: pass except Exception as e: e = traceback.format_exc() print("Error: ", e) # Returns the distance of two objects as coordinate system. # Distance calculated as difference between object 1 and object 2. def dist_two_objects(self, obj_1, obj_2): obj_1_pos = self.get_position(obj_1) obj_2_pos = self.get_position(obj_2) # Object distance in millimeters dist = [0] * 3 dist[0] = obj_1_pos[0] - obj_2_pos[0] dist[1] = obj_2_pos[1] - obj_1_pos[1] dist[2] = obj_1_pos[2] - obj_2_pos[2] return dist def get_translation(self, object): return object.getField('translation').getSFVec3f() def get_position(self, object): return object.getPosition() def get_rotation(self, object): return object.getOrientation() def populate_packets(self, no_packets): for i in no_packets: pass def create_pallets(self, num_pallets): # Create pallet list. self.pallet_list = [""] * num_pallets for i in range(num_pallets): pallet_name = "PALLET_" + str(i+1) pallet_obj = self.getFromDef(pallet_name) pallet = Pallet(pallet=pallet_obj, length=1.200, width=0.800, height=0.155) self.pallet_list[i] = pallet print("Pallet position ", i+1, ": ", self.pallet_list[i].get_pallet_position()) def place_pallets(self): #pallets = self.create_pallets(num_pallets=12) #self.pallet_list pass def get_pallet_pos(self, pallet_num): pass def get_packet_level(self, packet_num, packet_size, pallet_num): max_packets_level = 9 def set_small_dummy_pallets(self, pallet_num, num_packets, packet_size): # Set number of dummy packets. length = 0 width = 0 height = 0 if packet_size == "small": length = 0.300 width = 0.200 height = 0.180 elif packet_size == "large": length = 0.600 width = 0.400 height = 0.369 else: packet_size = "small" length = 0.300 width = 0.200 height = 0.180 for i in range(num_packets): packet_name = "PACKET_" + str(i+1) packet = Packet(length, width, height, packet_size, packet_name) self.pallet_list[pallet_num-1].add_packet(packet=packet) # Populate with packets # packet1 = Packet(0.300, 0.200, 0.180, "small") # packet2 = Packet(0.300, 0.200, 0.180, "small") # packet3 = Packet(0.300, 0.200, 0.180, "small") # packet4 = Packet(0.300, 0.200, 0.180, "small") # packet5 = Packet(0.300, 0.200, 0.180, "small") # packet6 = Packet(0.300, 0.200, 0.180, "small") # packet7 = Packet(0.300, 0.200, 0.180, "small") # packet8 = Packet(0.300, 0.200, 0.180, "small") # packet9 = Packet(0.300, 0.200, 0.180, "small") # packet10 = Packet(0.300, 0.200, 0.180, "small") # packet11 = Packet(0.300, 0.200, 0.180, "small") # packet12 = Packet(0.300, 0.200, 0.180, "small") # packet13 = Packet(0.300, 0.200, 0.180, "small") # packet14 = Packet(0.300, 0.200, 0.180, "small") # packet15 = Packet(0.300, 0.200, 0.180, "small") # packet16 = Packet(0.300, 0.200, 0.180, "small") # packet17 = Packet(0.300, 0.200, 0.180, "small") # packet18 = Packet(0.300, 0.200, 0.180, "small") # self.pallet_list[0].add_packet(packet=packet1) # self.pallet_list[0].add_packet(packet=packet2) # self.pallet_list[0].add_packet(packet=packet3) # self.pallet_list[0].add_packet(packet=packet4) # self.pallet_list[0].add_packet(packet=packet5) # self.pallet_list[0].add_packet(packet=packet6) # self.pallet_list[0].add_packet(packet=packet7) # self.pallet_list[0].add_packet(packet=packet8) # self.pallet_list[0].add_packet(packet=packet9) # self.pallet_list[0].add_packet(packet=packet10) # self.pallet_list[0].add_packet(packet=packet11) # self.pallet_list[0].add_packet(packet=packet12) # self.pallet_list[0].add_packet(packet=packet13) # self.pallet_list[0].add_packet(packet=packet14) # self.pallet_list[0].add_packet(packet=packet15) # self.pallet_list[0].add_packet(packet=packet16) # self.pallet_list[0].add_packet(packet=packet17) # self.pallet_list[0].add_packet(packet=packet18) # def check_dist def manual_control(self, keystrokes): # ------ MOVE AGV ----- # Increment AGV speed # self.increment_speed_agv(keystrokes=keystrokes) # Move AGV # self.emitter.send() # self.AGV_ROBOT_1.move_agv() # self.move_agv(keystrokes=keystrokes) # ------ HASSELHOFF HUG / SNAKE BOX - AXIS 1 ----- # Increment Axis 1 speed # self.increment_speed_snakebox(keystrokes=keystrokes) # Rotate snake box - Axis 1 # self.rotate_snakebox(keystrokes=keystrokes) # ------ TOWER - AXIS 2 ----- # Increment Tower speed # self.increment_speed_tower(keystrokes=keystrokes) # Move tower height # self.change_tower_height(keystrokes=keystrokes) # ------ SNAKE - AXIS 3 ----- # Increment Snake speed # self.increment_speed_snake_manual(keystrokes=keystrokes) # Move snake part 1 # self.move_snake_manual(keystrokes=keystrokes) pass # ----------------- COMMUNICATION --------------- # Receive message through receiver sent from emitter. # Received in utf-8 format. def receive_message(self): message = "" if self.receiver.getQueueLength() > 0: message = self.receiver.getData().decode('utf-8') self.receiver.nextPacket() # Splits the message. message = message.split() return message # Send message from emitter. # Sent in utf-8 format. def send_message(self, message): # message_sent = False if message != '':# and message != self.previous_message: self.previous_message = message # message_packed = struct.pack(message) # self.emitter.send(message_packed) self.emitter.send(message.encode('utf-8')) # message_sent = True # return message_sent def send_message_struct(self, message_list): # messages = [i for i in range(0,7)] # buffer = struct.pack('%sf' % len(message_list), *message_list) # var = struct.pack('hhl', 'test') s = struct.Struct('4s 4s 4s 4s 4s 4s 4s') packed = s.pack(message_list.encode('utf-8')) return packed def update_picked_packet_positions(self): updated = False if len(self.picked_packets) == 0: updated = False else: for i in range(len(self.picked_packets)): updated_pos = [0, 0, 0] # Get the packet and current position. packet_num = self.picked_packets[i] # print("Packet number: ", packet_num) packet_name = "PACKET_" + str(packet_num) packet = self.getFromDef(packet_name) current_pos = packet.getField('translation').getSFVec3f() # Get difference in current and previous gps position. gps_diff_x = self.current_gps_pos[0] - self.previous_gps_pos[0] gps_diff_y = self.current_gps_pos[1] - self.previous_gps_pos[1] gps_diff_z = self.current_gps_pos[2] - self.previous_gps_pos[2] gps_pos = [gps_diff_x, gps_diff_y, gps_diff_z] # Get updated packet position. for i in range(3): updated_pos[i] = current_pos[i] + gps_pos[i] # updated_pos_x = current_pos[0] + gps_pos[0] # updated_pos_y = current_pos[1] + gps_pos[1] # updated_pos_z = current_pos[2] + gps_pos[2] # Set updated packet position. packet.getField('translation').setSFVec3f(updated_pos) updated = True print("Updated position for: ", self.picked_packets) return updated # Convert list to string. List shall contain def list_to_string(self, list_elem): text_string = ' '.join([str(elem) for elem in list_elem]) return text_string def run(self): # Set initial conditions. self.pallet_num = 0 self.packet_num = 30 self.agv_packet_num = 0 # self.picking_started = True # Populate pallets with packets. self.set_small_dummy_pallets(pallet_num=(self.pallet_num + 1), num_packets=self.packet_num, packet_size="small") # Create a dummy pallet. #pallet1 = Pallet(pallet=self.pallet_list[0],length=1200, width=800, height=145) #pallet1.add_packet(packet1) #pallet1.add_packet(packet2) # print("Number of packets: ", self.pallet_list[0].get_num_packets()) # print("Packet num 1: ", self.pallet_list[0].get_packet(1)) # print("Packets: ", self.pallet_list[self.pallet_num].get_packets()) # print("Num packets: ", sum(1 for _ in filter(None.__ne__, self.pallet_list[0].get_packets()))) # print("Num packets: ", sum(x is not None for x in self.pallet_list[0].get_num_packets())) # print("Num packets: ", self.pallet_list[self.AGV_PALLET_NO].get_num_packets()) # print("Num packets pallet 0: ", self.pallet_list[0].get_num_packets()) # print("Packets pallet 0: ", self.pallet_list[0].get_packets()) # print("Num packets pallet 1: ", self.pallet_list[1].get_num_packets()) # print("Packets pallet 1: ", self.pallet_list[1].get_packets()) # print("Num packets pallet 2: ", self.pallet_list[2].get_num_packets()) # print("Packets pallet 2: ", self.pallet_list[2].get_packets()) # print("Packet deleted: ", self.pallet_list[0].remove_packet(1)) # print("Packets: ", self.pallet_list[0].get_packets()) # Position data for connecting packets. positioned = [False for i in range(3)] error_margin = [0] * 3 error_margin[0] = 0.1 error_margin[1] = 0.025 error_margin[2] = 0.05 # Robot mode selection mode_selection = 2 mode = self.robot_mode(mode_selection) # Initial robot state # self.state_machine("") # Main loop: # while self.step(timestep) != -1: while True: # Read keyboard values keystrokes = self.read_keyboard() # Update GPS positions. self.current_gps_pos = self.getFromDef('body_agv').getPosition() # print("Current gps pos of AGV: ", self.current_gps_pos) # print("==================") # print("AGV position 2: ", self.get_position(self.AGV_ROBOT_1)) # print("Suction cup position: ", self.get_position(self.SUCTION_CUP)) # print("==================") dist = self.dist_two_objects(self.AGV_ROBOT_1, self.SUCTION_CUP) # print("Difference AGV and Snake tip: ", dist) # print(self.get_position(self.SUCTION_CUP)) # test = self.getFromDef('body_agv') # print("Parent node: ", test.getPosition()) suction_cup_pos = self.get_position(self.SUCTION_CUP) # packet_picked_pos_init = self.pallet_list[0].get_packet_position(16) # print("Suction cup position: ", suction_cup_pos) # print("Packet picked position: ", packet_picked_pos_init) # print(positioned) #self.switch_case(argument=1) # Runs either manual by keyboard input, or in automatic or remote mode. if mode == 'Manual mode': manual_message = ['None'] * 12 # Clear previous message. message = self.receive_message() # print("Message received supervisor:", message) # AGV keyboard input. if (self.FORWARD in keystrokes): # Drive forwards if (self.TURN_LEFT in keystrokes): # Turn left agv_state = 'forward_left' elif (self.TURN_RIGHT in keystrokes): # Turn right agv_state = 'forward_right' else: agv_state = 'forward' elif (self.BACKWARD in keystrokes): # Drive backwards if (self.TURN_LEFT in keystrokes): # Turn left agv_state = 'backward_left' elif (self.TURN_RIGHT in keystrokes): # Turn right agv_state = 'backward_right' else: agv_state = 'backward' elif (self.TURN_LEFT in keystrokes): # Turn left agv_state = 'left' elif (self.TURN_RIGHT in keystrokes): # Turn right agv_state = 'right' else: agv_state = 'idle' manual_message[0] = agv_state # Tower keyboard input. if (self.UP_AXIS_2 in keystrokes): tower_state = 'up' elif (self.DOWN_AXIS_2 in keystrokes): tower_state = 'down' else: tower_state = 'idle' manual_message[5] = tower_state # Snakebox keyboard input. if (self.LEFT_AXIS_1 in keystrokes): snakebox_state = 'left' elif (self.RIGHT_AXIS_1 in keystrokes): snakebox_state = 'right' else: snakebox_state = 'idle' manual_message[4] = snakebox_state # Snake keyboard input. if (self.EXTEND_AXIS_3 in keystrokes): snake_state = 'extend' elif (self.RETRACT_AXIS_3 in keystrokes): snake_state = 'retract' else: snake_state = 'idle' manual_message[6] = snake_state # Snaketip keyboard input. if (self.LEFT_AXIS_4 in keystrokes): snaketip_state = 'left' elif (self.RIGHT_AXIS_4 in keystrokes): snaketip_state = 'right' else: snaketip_state = 'idle' manual_message[7] = snaketip_state # if (self.axis_2_pos > self.motor_axis_2.getMaxPosition()): # self.axis_2_pos = round(self.motor_axis_2.getMaxPosition(), 2) # # print("Desired pos: ", self.axis_2_pos) # # print("Maximum position: ", motor_axis_2.getMaxPosition()) # sys.stderr.write("Axis 2 has reached maximum height.\n") # elif (self.axis_2_pos < self.motor_axis_2.getMinPosition()): # self.axis_2_pos = round(self.motor_axis_2.getMinPosition(), 2) # # print("Min. pos: ", motor_axis_2.getMaxPosition()) # sys.stderr.write("Axis 2 has reached minimum height.\n") # else: # self.motor_axis_2.setPosition(self.axis_2_pos) self.emitted_message = self.list_to_string(manual_message) elif mode == "Remote mode": remote_message = ['None'] * 20 # Clear previous message. message = self.receive_message() # print("Message received supervisor:", message) # print("=========================================") # print("OTHER pallet packets: ", self.pallet_list[self.pallet_num].get_packets()) # print(" -------------- ") # print("AGV pallet packets: ", self.pallet_list[self.AGV_PALLET_NO].get_packets()) # print("=========================================") # robot_status = 'true' # message[2].lower() try: self.robot_status = message[2].lower() self.picking_started = True except: pass print("Currently packed: ", self.picked_packets) if self.packet_num <= 0: self.picking_finished = True else: # Picked packets. packet_picked_pos_init = self.pallet_list[self.pallet_num].get_packet_position(self.packet_num) remote_message[8] = packet_picked_pos_init[0] remote_message[9] = packet_picked_pos_init[1] remote_message[10] = packet_picked_pos_init[2] # pallet_num = 0 # # packet_num = 16 # print("AGV Pallet: ", "Num. packets: ", self.pallet_list[self.AGV_PALLET_NO].get_num_packets(), # " and stored: ", self.pallet_list[self.AGV_PALLET_NO].get_packets()) # print("Main Pallet: ", "Num. packets: ", self.pallet_list[self.pallet_num].get_num_packets(), # " and stored: ", self.pallet_list[self.pallet_num].get_packets()) if self.picking_started and not self.picking_finished: if (self.robot_status == 'true'): # print("This is remaining packets: ", self.pallet_list[self.pallet_num].get_packets()) if all(positioned): if (message[0] == 'attach'): # print("We're about to connect!") packet_name = self.pallet_list[self.pallet_num].get_packet(self.packet_num).get_name() packet = self.getFromDef(packet_name) x = round(suction_cup_pos[0], 7) y = round(suction_cup_pos[1], 7) - self.pallet_list[self.pallet_num].get_packet(self.packet_num).get_height() / 2 z = round(suction_cup_pos[2], 7) pos = [x, y, z] packet.getField('translation').setSFVec3f(pos) # print("New packet position: ", self.get_position(packet)) # print("Used position: ", pos) # packet_picked_pos # setSFVec3f(suction_cup_pos) remote_message[11] = 'True' print("Picked packet: ", packet_name) print("Picked packet number: ", self.packet_num) # print("We're connected!") # print("=============") # Rotate the package in accordance with the snake angle. gripper_angle = math.radians(float(message[1])) gripper_angle_list = [0, 1, 0, gripper_angle] # gripper = self.getFromDef('motor_axis_4') # gripper_angle_1 = gripper.getField('rotation') # print("Gripper angle first: ", gripper_angle_1) # gripper_angle_2 = gripper.getField('rotation').getSFRotation() # print("Gripper angle first: ", gripper_angle_2) # gripper_angle = gripper.getSFRotation() # print("Gripper angle: ", gripper_angle) packet.getField('rotation').setSFRotation(gripper_angle_list) # Sets status that packet has been picked. remote_message[11] = "True" if not self.packet_added: # Packet to be moved. packet = self.pallet_list[self.pallet_num].get_packet(packet_num=self.packet_num) self.picked_packets.append(self.packet_num) print("PACKET ADDED: ", packet) # Find position on new pallet and place packet. # self.pallet_list[self.AGV_PALLET_NO].add_packet(packet=packet) self.agv_packet_num += 1 # self.agv_packet_num = self.pallet_list[self.AGV_PALLET_NO].get_num_packets() # Packet has been added. self.packet_added = True if self.packet_added: # print("AGV pallet packets: ", self.pallet_list[self.AGV_PALLET_NO].get_packets()) # print("Other pallet packets: ", self.pallet_list[self.pallet_num].get_packets()) # Get the next position for the packet - Only preliminary position. AGV to decide final position. # packet_picked_pos_final = self.pallet_list[self.AGV_PALLET_NO].get_packet_position(self.agv_packet_num) # remote_message[12] = packet_picked_pos_final[0] # remote_message[13] = packet_picked_pos_final[1] # remote_message[14] = packet_picked_pos_final[2] remote_message[15] = self.agv_packet_num packet_dimensions = self.pallet_list[self.pallet_num].get_packet_dimensions(self.packet_num) # packet_dimensions = self.pallet_list[self.AGV_PALLET_NO].get_packet_dimensions(self.agv_packet_num) remote_message[16] = packet_dimensions[0] # Width remote_message[17] = packet_dimensions[1] # Length remote_message[18] = packet_dimensions[2] # Height # print("Packet dimensions: ", "Width = ", remote_message[16], "Length = ", remote_message[17], "Height = ", remote_message[18]) # Ready to do a count when detaching packet. self.count_now = True elif (message[0] == 'detach'): if self.count_now: if not self.packet_removed: # Remove packet from initial pallet. # self.pallet_list[self.pallet_num].remove_packet(packet_num=self.packet_num) self.packet_removed = True self.packet_num -= 1 print("I counted now up to: ", self.packet_num) self.count_now = False self.packet_added = False remote_message[11] = "False" elif (message[0] == 'idle'): remote_message[11] = "False" print("Currently I have: ", self.packet_num, " packets.") else: for i in range(3): # Check x, y and z position if suction_cup_pos[i] >= (packet_picked_pos_init[i] - error_margin[i]) and suction_cup_pos[i] <= \ (packet_picked_pos_init[i] + error_margin[i]): positioned[i] = True else: positioned[i] = False else: print("AGV not ready.") elif self.picking_finished: print("Picking order has finished.") remote_message[0] = 'idle' else: print("AGV robot is not ready.") self.picking_started = False # Updating packet position on AGV. if self.update_picked_packet_positions(): print("Packet positions on AGV updated.") # print("Packet position: ", positioned) self.emitted_message = self.list_to_string(remote_message) elif mode == "Automatic mode": pass else: # Print error sys.stderr.write("No or incorrect mode selected.\n") # The program will exit sys.exit("The program will now be terminated.") pass # time1 = time.time() # count = 0 # for i in range(100): # count += 1 # s = self.emitted_message+str(count) # self.send_message(s) # time2 = time.time() # print("Time 1: ", time1) # print("Time 2: ", time2) # print("========") # print("Keys: ", keystrokes) # print("Type: ", type(keystrokes[0])) # print(*keystrokes[0]) # bufferTest = self.send_message_struct(keystrokes) # Send the message at the end of the iteration. self.send_message(self.emitted_message) # Update previous GPS position. self.previous_gps_pos = self.current_gps_pos try: pass # print(self.pallet_list[0].get_pallet_position()) # print(self.pallet_list[0].get_packet_position(16)) except AttributeError: print("Pallet number is incorrect.") # Reset emitted message. self.emitted_message = "" # Breaks the simulation. while self.step(self.TIME_STEP) == -1: print("============== I'm breaking up! ==============") break def switch_case_robot(self, argument): # Creating a dictionary of the states. switcher = { 1: self.robot_idle_sc, 2: self.move_agv_sc, 3: self.move_tower_sc, 4: self.rotate_snakebox_sc, 5: self.extend_snake_sc, 6: self.attach_packet_sc, 7: self.detach_packet_sc, 8: self.retract_snake_sc } # Get the function from switcher dictionary func = switcher.get(argument, lambda: "Invalid robot state") # Execute the function state = func() return state def robot_idle_sc(self): print("Robot is in idle mode!") pass def move_agv_sc(self): pass def move_tower_sc(self): pass def rotate_snakebox_sc(self): pass def extend_snake_sc(self): pass def attach_packet_sc(self): pass def detach_packet_sc(self): pass def retract_snake_sc(self): pass # ------ Switch case ----- def manual_mode_sc(self): return "Manual mode" def remote_mode_sc(self): return "Remote mode" def automatic_mode_sc(self): return "Automatic mode" # Set the type of robot mode. def robot_mode(self, argument): switcher = { 1: self.manual_mode_sc, 2: self.remote_mode_sc, 3: self.automatic_mode_sc } # Get the function from switcher dictionary func = switcher.get(argument, lambda: "Invalid robot mode") # Execute the function mode = func() return mode
from propeller_keyboard_helper import Propeller_Simulation, TIME_STEP from controller import Keyboard import time keyboard = Keyboard() keyboard.enable(TIME_STEP) sim = Propeller_Simulation() while True: key = keyboard.getKey() if key == ord('A'): sim.set_velocity(0.6) elif key == ord('D'): sim.set_velocity(-0.6) else: sim.set_velocity(0) time.sleep(1/TIME_STEP)
def initialize_webots_keyboard(self): """ Initialize webots keyboard """ self.key_press = Keyboard() self.key_press.enable(self.TIMESTEP * 25)
def __init__(self, joints): self.msg = """ Reading from the keyboard and Publishing to Twist! --------------------------- Moving around: u i o j k l m , . For Holonomic mode (strafing), hold down the shift key: --------------------------- U I O J K L M < > t : up (+z) b : down (-z) anything else : stop q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% CTRL-C to quit """ self.moveBindings = { '73':(1,0,0,0), '79':(1,0,0,-1), '74':(0,0,0,1), '76':(0,0,0,-1), '85':(1,0,0,1), '44':(-1,0,0,0), '46':(-1,0,0,1), '77':(-1,0,0,-1), '65615':(1,-1,0,0), '65609':(1,0,0,0), '65610':(0,1,0,0), '65612':(0,-1,0,0), '65621':(1,1,0,0), '65596':(-1,0,0,0), '65598':(-1,-1,0,0), '65613':(-1,1,0,0), '84':(0,0,1,0), '66':(0,0,-1,0), } self.speedBindings = { '81':(1.1,1.1), '90':(.9,.9), '87':(1.1,1), '88':(.9,1), '69':(1,1.1), '67':(1,.9), } self.keyboard = Keyboard() self.keyboard.enable(32) self.speed = 0.5 self.turn = 1 self.x = 0 self.y = 0 self.th = 0 self.status = 0 self.twist = [0, 0, 0] # Base parameters self.A = 74.87/1000 # Wheel width self.B = 100.0/1000 # Wheel diameter self.C = 471.0/1000 # Between from and rear wheels self.D = 300.46/1000 # Base width self.E = 28.0/1000 # Roll diameter self.slideRatio = 1 self.joints = joints print(self.msg) print("currently:\tspeed %s\tturn %s " % (self.speed, self.turn))
def run(self): # initate lidar timeStep = 64 lidar = self.getDevice('LDS-01') lidar.enable(timeStep) # lidarWidth = lidar.getHorizontalResolution() """Set the Pedestrian pose and position.""" opt_parser = optparse.OptionParser() opt_parser.add_option("--trajectory", default="", help="Specify the trajectory in the format [x1 y1, x2 y2, ...]") opt_parser.add_option("--speed", type=float, help="Specify walking speed in [m/s]") opt_parser.add_option("--step", type=int, help="Specify time step (otherwise world time step is used)") opt_parser.add_option("--caneMovement", type=int, help="Should the cane move sideways?") options, args = opt_parser.parse_args() if not options.trajectory or len(options.trajectory.split(',')) < 2: print("You should specify the trajectory using the '--trajectory' option.") print("The trajectory shoulld have at least 2 points.") return if options.speed and options.speed > 0: self.speed = options.speed if options.step and options.step > 0: self.time_step = options.step else: self.time_step = int(self.getBasicTimeStep()) if options.caneMovement == 0: self.caneMovement = bool(options.caneMovement) point_list = options.trajectory.split(',') self.number_of_waypoints = len(point_list) self.waypoints = [] for i in range(0, self.number_of_waypoints): self.waypoints.append([]) self.waypoints[i].append(float(point_list[i].split()[0])) self.waypoints[i].append(float(point_list[i].split()[1])) self.root_node_ref = self.getSelf() self.root_translation_field = self.root_node_ref.getField("translation") self.root_rotation_field = self.root_node_ref.getField("rotation") for joint in self.joint_names: self.joints_position_field.append(self.root_node_ref.getField(joint)) # compute waypoints distance self.updateWaypointsDistance() # enable keyboard keyboard = Keyboard() keyboard.enable(self.time_step) # main cycle while not self.step(self.time_step) == -1: # lidar # inf if nothing, outputs a float if there is a solid object in way imageData = lidar.getRangeImage() # choose number of partitions and field of view partitionMinima, partitionBorders = lou.findPartitionMinima(imageData, 5, 180) print("Border angles of partitions: " + str (partitionBorders)) print("Closest distance for each partition: " + str(["%.2f"%x for x in partitionMinima])) # choose min and max values for distance to be detected and converted to feedback # LDS-01 range on manufacturer website: 0.12m ~ 3.5m feedbackLevels = lou.getFeedbackLevels(partitionMinima, 0.5, 3) print("Feedback levels for each partition: " + str(["%.2f"%x for x in feedbackLevels])) if not self.clearLidarFeedback: self.commandOutput = feedbackLevels # print table lou.printFeedbackLevels(self.commandOutput, 5) # listen for keyboard press key = keyboard.getKey() # action: voice recognition if (key == ord('V')): self.buttonPressed = True self.clearLidarFeedback = True commands = ["where is location one", "check the battery"] phrases = [(command, 1.0) for command in commands] locations = {} locations["location one"] = self.locationOne command = listen(phrases) if command != "": executionOutput = executeCommand(command, locations, {}) if isinstance(executionOutput, int): self.commandOutput = battery_to_haptic(executionOutput) elif isinstance(executionOutput, list): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] self.commandOutput = haptic_guide(currentLocation, executionOutput, -currentAngle - self.relativeCaneAngle) # action: show and go to location if (key == ord('G')): self.buttonPressed = True self.clearLidarFeedback = True currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = self.locationOne self.commandOutput = haptic_guide(currentLocation, newDestination, -currentAngle - self.relativeCaneAngle) self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: show location if (key == ord('H')): self.buttonPressed = True self.clearLidarFeedback = True currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = self.locationOne self.commandOutput = haptic_guide(currentLocation, newDestination, -currentAngle - self.relativeCaneAngle) # action: stop if (key == ord(' ')): self.buttonPressed = True # action: show battery if (key == ord('B')): self.buttonPressed = True self.clearLidarFeedback = True self.commandOutput = battery_to_haptic(35) if (key == ord(' ')): self.buttonPressed = True # action: turn right 90 if (key == ord('D')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(-180 - currentAngle)), z + 100 * math.sin(math.radians(-180 - currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: turn right 45 if (key == ord('E')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(-225 - currentAngle)), z + 100 * math.sin(math.radians(-225 - currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: turn left 90 if (key == ord('A')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(-currentAngle)), z + 100 * math.sin(math.radians(-currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: turn left 45 if (key == ord('Q')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(45 - currentAngle)), z + 100 * math.sin(math.radians(45 - currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: turn back 180 if (key == ord('S')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(-90 - currentAngle)), z + 100 * math.sin(math.radians(-90 - currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # action: continue further forward 0 if (key == ord('W')): currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3]) x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]] newDestination = [x + 100 * math.cos(math.radians(-270 - currentAngle)), z + 100 * math.sin(math.radians(-270 - currentAngle))] self.add_new_current_waypoint = True self.new_current_waipoint = newDestination # get front partition that relative to the user relativeCaneAngle = 0 for i in [7, 8]: relativeCaneAngle += self.joints_position_field[i].getSFFloat() self.relativeCaneAngle = math.degrees(relativeCaneAngle) relativeFrontPartitionIndex = [idx for idx, border in enumerate(partitionBorders) if border > relativeCaneAngle][0] - 1 # following stops on any feedback in any of the patitions #if any(map(lambda x: x > 0, feedbackLevels)): if feedbackLevels[relativeFrontPartitionIndex] > self.stopMovingFeedbackLevel or self.buttonPressed: # stop movement, reset delay self.stopMoving = True self.delayMoving = False self.buttonPressed = False self.timeNotMoving += self.timeAfterDelayStart self.delayStartTimestamp = 0 self.timeAfterDelayStart = 0 if self.stopTimestamp == 0: self.stopTimestamp = self.getTime() else: # path is clear if self.delayMoving: self.timeAfterDelayStart = self.getTime() - self.delayStartTimestamp if self.timeAfterDelayStart >= self.resumeMovingDelay: # delay ended, resume movement self.timeNotMoving += self.timeAfterDelayStart self.delayMoving = False self.clearLidarFeedback = False self.delayStartTimestamp = 0 self.timeAfterDelayStart = 0 if self.stopMoving: # path just got cleared, start delay self.timeNotMoving += self.getTime() - self.stopTimestamp self.stopMoving = False self.stopTimestamp = 0 self.delayMoving = True self.delayStartTimestamp = self.getTime() # internal total runtime of robot time = self.getTime() - self.timeNotMoving if ((not self.stopMoving) and (not self.delayMoving)): # free to move current_sequence = int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO) % self.WALK_SEQUENCES_NUMBER) # compute the ratio 'distance already covered between way-point(X) and way-point(X+1)' # / 'total distance between way-point(X) and way-point(X+1)' ratio = (time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO - \ int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO)) # body parts movement for i in range(0, len(self.angles)): if i in [3,4,6]: current_angle = self.angles[i][0] self.joints_position_field[i].setSFFloat(current_angle) elif (not self.caneMovement and i in [5,7,8]): if i in [7,8]: current_angle = 0 else: current_angle = self.angles[i][1] self.joints_position_field[i].setSFFloat(current_angle) else: current_angle = self.angles[i][current_sequence] * (1 - ratio) + \ self.angles[i][(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio self.joints_position_field[i].setSFFloat(current_angle) # adjust height self.current_height_offset = self.height_offsets[current_sequence] * (1 - ratio) + \ self.height_offsets[(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio # total distance covered distance = time * self.speed # subtract distance covered in previous full cycles relative_distance = distance - int(distance / self.waypoints_distance[self.number_of_waypoints - 1]) * \ self.waypoints_distance[self.number_of_waypoints - 1] # find current waypoint that we are heading to for i in range(0, self.number_of_waypoints): if self.waypoints_distance[i] > relative_distance: break # if new waypoint is to be added if self.add_new_current_waypoint: self.add_new_current_waypoint = False self.number_of_waypoints += 2 current_translation = self.root_translation_field.getSFVec3f() x = current_translation[0] z = current_translation[2] self.waypoints = self.waypoints[:i + 1] + [[x, z]] + [self.new_current_waipoint] + self.waypoints[i + 1:] # calculate the new waypoint distances list self.updateWaypointsDistance() # note: relative distance does not need to be updated after waypoint is added # how much distance is covered between the last and current waypoint distance_ratio = 0 if i == 0: distance_ratio = relative_distance / self.waypoints_distance[0] else: distance_ratio = (relative_distance - self.waypoints_distance[i - 1]) / \ (self.waypoints_distance[i] - self.waypoints_distance[i - 1]) # new coordinates x = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][0] + \ (1 - distance_ratio) * self.waypoints[i][0] z = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][1] + \ (1 - distance_ratio) * self.waypoints[i][1] root_translation = [x, self.ROOT_HEIGHT + self.current_height_offset, z] # new angle angle = math.atan2(self.waypoints[(i + 1) % self.number_of_waypoints][0] - self.waypoints[i][0], self.waypoints[(i + 1) % self.number_of_waypoints][1] - self.waypoints[i][1]) rotation = [0, 1, 0, angle] # update position and rotation self.root_translation_field.setSFVec3f(root_translation) self.root_rotation_field.setSFRotation(rotation)
def WithNoise(input_vector): mean = 0 std = 0.005 n = len(input_vector) noise = np.random.normal(mean,std,n) return list(np.array(input_vector) + noise) dir_path = os.path.dirname(os.path.realpath(__file__)) L = Logger() env = Mitsos(ACTIONS='CONTINUOUS') state = env.reset() keyboard = Keyboard() # to control training from keyboard input keyboard.enable(env.timestep) # # USING BOTH CAMERA AND IR SENSORS # n_inputs = 2 # dqn = ComplexDQN # mem = MemoryDouble # USING ONLY IR SENSORS n_inputs = 1 dqn = SimpleDQN mem = Memory # # USING ONLY CAMERA # n_inputs = 1 # dqn = ConvDQN
class BaseController: def __init__(self, joints): self.msg = """ Reading from the keyboard and Publishing to Twist! --------------------------- Moving around: u i o j k l m , . For Holonomic mode (strafing), hold down the shift key: --------------------------- U I O J K L M < > t : up (+z) b : down (-z) anything else : stop q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% CTRL-C to quit """ self.moveBindings = { '73':(1,0,0,0), '79':(1,0,0,-1), '74':(0,0,0,1), '76':(0,0,0,-1), '85':(1,0,0,1), '44':(-1,0,0,0), '46':(-1,0,0,1), '77':(-1,0,0,-1), '65615':(1,-1,0,0), '65609':(1,0,0,0), '65610':(0,1,0,0), '65612':(0,-1,0,0), '65621':(1,1,0,0), '65596':(-1,0,0,0), '65598':(-1,-1,0,0), '65613':(-1,1,0,0), '84':(0,0,1,0), '66':(0,0,-1,0), } self.speedBindings = { '81':(1.1,1.1), '90':(.9,.9), '87':(1.1,1), '88':(.9,1), '69':(1,1.1), '67':(1,.9), } self.keyboard = Keyboard() self.keyboard.enable(32) self.speed = 0.5 self.turn = 1 self.x = 0 self.y = 0 self.th = 0 self.status = 0 self.twist = [0, 0, 0] # Base parameters self.A = 74.87/1000 # Wheel width self.B = 100.0/1000 # Wheel diameter self.C = 471.0/1000 # Between from and rear wheels self.D = 300.46/1000 # Base width self.E = 28.0/1000 # Roll diameter self.slideRatio = 1 self.joints = joints print(self.msg) print("currently:\tspeed %s\tturn %s " % (self.speed, self.turn)) def getKey(self): key = self.keyboard.getKey() return str(key) def vels(self): return "currently:\tspeed %s\tturn %s " % (self.speed, self.turn) def getTwist(self): try: key = self.getKey() if key in self.moveBindings.keys(): self.x = self.moveBindings[key][0] self.y = self.moveBindings[key][1] self.th = self.moveBindings[key][3] elif key in self.speedBindings.keys(): self.speed = self.speed * self.speedBindings[key][0] self.turn = self.turn * self.speedBindings[key][1] print(self.vels()) if (self.status == 14): print(self.msg) self.status = (self.status + 1) % 15 else: self.x = 0 self.y = 0 self.th = 0 self.twist[0] = self.x*self.speed self.twist[1] = self.y*self.speed self.twist[2] = self.th*self.turn return except Exception as e: print(e) self.twist[0] = 0; self.twist[1] = 0; self.twist[2] = 0; def getMotorVelocities(self): velFromX = self.twist[2]/self.B velFromY = self.twist[1]/(self.B*self.slideRatio) velFromTheta = self.twist[0] * (self.C + self.D) / self.B wheelVelocities = [0, 0, 0, 0] wheelVelocities[0] = velFromX + velFromY + velFromTheta wheelVelocities[1] = -velFromX - velFromY + velFromTheta wheelVelocities[2] = velFromX - velFromY + velFromTheta wheelVelocities[3] = -velFromX + velFromY + velFromTheta return wheelVelocities; def rotateBaseMotors(self): self.getTwist() #print(self.twist) wheelVelocities = self.getMotorVelocities() self.joints[0].setVelocity(wheelVelocities[0]) self.joints[1].setVelocity(wheelVelocities[1]) self.joints[2].setVelocity(wheelVelocities[2]) self.joints[3].setVelocity(wheelVelocities[3])
driver = Driver() basicTimeStep = int(driver.getBasicTimeStep()) TIME_STEP = 100 UNKNOWN = 99999.9 FILTER_SIZE = 3 steering_angle = 0.0 KP = 0.25 KI = 0.00 KD = 2 gps_coords = [0.0 for i in range(3)] gps_speed = 0.0 speed = 0 sick_fov = -1.0 keyboard = Keyboard() def setSpeed(kmh): global speed speed = 250.0 if kmh > 250.0 else kmh print("setting speed to {} km/h".format(speed)) driver.setCruisingSpeed(speed) def set_autodrive(onoff): global autodrive if autodrive == onoff: return autodrive = onoff if autodrive == False:
class CustomRobotClass: def __init__(self): # Initialize Robot self.robot = Robot() self.time_step = int(self.robot.getBasicTimeStep()) print("Robot time step ", self.time_step) # Initialize sensors self.metacamera = self.robot.getCamera("MultiSense S21 meta camera") self.leftcamera = self.robot.getCamera("MultiSense S21 left camera") self.rightcamera = self.robot.getCamera("MultiSense S21 right camera") self.rangeFinder = self.robot.getRangeFinder( "MultiSense S21 meta range finder") self.cameras = { "left": self.leftcamera, "right": self.rightcamera, "meta": self.metacamera, "depth": self.rangeFinder } self.leftpositionsensor = self.robot.getPositionSensor( 'back left wheel sensor') self.rightpositionsensor = self.robot.getPositionSensor( 'back right wheel sensor') self.keyboard = Keyboard() self.pen = self.robot.getPen('pen') # Enable sensors self.metacamera.enable(self.time_step) self.leftcamera.enable(self.time_step) self.rightcamera.enable(self.time_step) self.rangeFinder.enable(self.time_step) self.leftpositionsensor.enable(self.time_step) self.rightpositionsensor.enable(self.time_step) self.keyboard.enable(self.time_step) # Initialize wheels backrightwheel = self.robot.getMotor('back right wheel') backleftwheel = self.robot.getMotor('back left wheel') frontrightwheel = self.robot.getMotor('front right wheel') frontleftwheel = self.robot.getMotor('front left wheel') self.wheels = [ backrightwheel, backleftwheel, frontrightwheel, frontleftwheel ] self.set_velocity_control() self.prevlspeed = 0 self.prevrspeed = 0 self.curlspeed = 0 self.currspeed = 0 def set_velocity_control(self): for wheel in self.wheels: wheel.setPosition((float('inf'))) wheel.setVelocity(0) def step(self): self.robot.step(self.time_step) def set_speed(self, l, r): print("Inside set speed", SPEED_UNIT * l, SPEED_UNIT * r) if self.prevlspeed != l or self.prevrspeed != r: self.wheels[0].setVelocity(SPEED_UNIT * r) self.wheels[1].setVelocity(SPEED_UNIT * l) self.wheels[2].setVelocity(SPEED_UNIT * r) self.wheels[3].setVelocity(SPEED_UNIT * l) self.prevlspeed = self.curlspeed self.prevrspeed = self.currspeed self.curlspeed = l self.currspeed = r def get_image(self, camera_name, depth_option=False): if (depth_option == True): return self.cameras[camera_name].getRangeImageArray() else: return self.cameras[camera_name].getImageArray()
""" Webots controller to manually drive a car using the arrow keys. Authors: John Shamoon and Wisam Bunni """ from numpy import pi from controller import Keyboard from vehicle import Driver ego_vehicle = Driver() keyboard = Keyboard() keyboard.enable(int(ego_vehicle.getBasicTimeStep())) SPEED = 25 FORWARD_RIGHT = pi / 8 FORWARD_LEFT = -FORWARD_RIGHT BACKWARD_RIGHT = -FORWARD_RIGHT BACKWARD_LEFT = -FORWARD_LEFT RIGHT = pi / 4 LEFT = -RIGHT FORWARD = 0 BACKWARD = -FORWARD NEUTRAL = 0 SIGNALS = { (-1, -1): (NEUTRAL, NEUTRAL), (keyboard.LEFT, -1): (NEUTRAL, LEFT), (-1, keyboard.LEFT): (NEUTRAL, LEFT), (keyboard.RIGHT, -1): (NEUTRAL, RIGHT), (-1, keyboard.RIGHT): (NEUTRAL, RIGHT), (keyboard.UP, -1): (SPEED, FORWARD), (-1, keyboard.UP): (SPEED, FORWARD),
def retrieveData(): myClient = pymongo.MongoClient( "mongodb+srv://test:[email protected]/directions?retryWrites=true&w=majority" ) myDb = myClient["directions"] myCol = myDb["directions"] x = myCol.find() print(x[0]) TIME_STEP = 64 robot = Robot() timestep = int(robot.getBasicTimeStep()) keyboard = Keyboard() keyboard.enable(timestep) ds = [] dsNames = ['ds_right', 'ds_left'] for i in range(2): ds.append(robot.getDistanceSensor(dsNames[i])) ds[i].enable(TIME_STEP) wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(robot.getMotor(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) while robot.step(TIME_STEP) != -1: _, fr = video.read()
class SalamanderCMC(object): """Salamander robot for CMC""" N_BODY_JOINTS = 10 N_LEGS = 4 def __init__(self, robot, n_iterations, parameters, logs="logs/log.npz"): super(SalamanderCMC, self).__init__() self.robot = robot timestep = int(robot.getBasicTimeStep()) self.network = SalamanderNetwork(1e-3*timestep, parameters) # Position sensors self.position_sensors = [ self.robot.getPositionSensor('position_sensor_{}'.format(i+1)) for i in range(self.N_BODY_JOINTS) ] + [ self.robot.getPositionSensor('position_sensor_leg_{}'.format(i+1)) for i in range(self.N_LEGS) ] for sensor in self.position_sensors: sensor.enable(timestep) # GPS self.gps = robot.getGPS("fgirdle_gps") self.gps.enable(timestep) # Get motors self.motors_body = [ self.robot.getMotor("motor_{}".format(i+1)) for i in range(self.N_BODY_JOINTS) ] self.motors_legs = [ self.robot.getMotor("motor_leg_{}".format(i+1)) for i in range(self.N_LEGS) ] # Set motors for motor in self.motors_body: motor.setPosition(0) motor.enableForceFeedback(timestep) motor.enableTorqueFeedback(timestep) for motor in self.motors_legs: motor.setPosition(-np.pi/2) motor.enableForceFeedback(timestep) motor.enableTorqueFeedback(timestep) # Iteration counter self.iteration = 0 # Logging self.log = ExperimentLogger( n_iterations, n_links=1, n_joints=self.N_BODY_JOINTS+self.N_LEGS, filename=logs, timestep=1e-3*timestep, **parameters ) #GPS stuff self.waterPosx = 0 self.NetworkParameters = self.network.parameters self.SimulationParameters = parameters self.doTransition = False self.keyboard = Keyboard() self.keyboard.enable(samplingPeriod=100) self.lastkey = 0 def log_iteration(self): """Log state""" self.log.log_link_positions(self.iteration, 0, self.gps.getValues()) for i, motor in enumerate(self.motors_body): # Position self.log.log_joint_position( self.iteration, i, self.position_sensors[i].getValue() ) # # Velocity # self.log.log_joint_velocity( # self.iteration, i, # motor.getVelocity() # ) # Command self.log.log_joint_cmd( self.iteration, i, motor.getTargetPosition() ) # Torque self.log.log_joint_torque( self.iteration, i, motor.getTorqueFeedback() ) # Torque feedback self.log.log_joint_torque_feedback( self.iteration, i, motor.getTorqueFeedback() ) for i, motor in enumerate(self.motors_legs): # Position self.log.log_joint_position( self.iteration, 10+i, self.position_sensors[10+i].getValue() ) # # Velocity # self.log.log_joint_velocity( # self.iteration, i, # motor.getVelocity() # ) # Command self.log.log_joint_cmd( self.iteration, 10+i, motor.getTargetPosition() ) # Torque self.log.log_joint_torque( self.iteration, 10+i, motor.getTorqueFeedback() ) # Torque feedback self.log.log_joint_torque_feedback( self.iteration, 10+i, motor.getTorqueFeedback() ) def step(self): """Step""" # Increment iteration self.iteration += 1 # Update network self.network.step() positions = self.network.get_motor_position_output() # Update control for i in range(self.N_BODY_JOINTS): self.motors_body[i].setPosition(positions[i]) for i in range(self.N_LEGS): self.motors_legs[i].setPosition( positions[self.N_BODY_JOINTS+i] - np.pi/2 ) key=self.keyboard.getKey() if (key==ord('A') and key is not self.lastkey): print('Turning left') self.SimulationParameters.turnRate = [0.5,1] self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters) self.lastkey = key if (key==ord('D') and key is not self.lastkey): print('Turning right') self.SimulationParameters.turnRate = [1,0.5] self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters) self.lastkey = key if (key==ord('W') and key is not self.lastkey): print('Going forward') self.SimulationParameters.turnRate = [1,1] self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters) self.SimulationParameters.Backwards = False self.NetworkParameters.set_phase_bias(self.SimulationParameters) self.lastkey = key if (key==ord('S') and key is not self.lastkey): print('Going backward') self.SimulationParameters.Backwards = True self.NetworkParameters.set_phase_bias(self.SimulationParameters) self.SimulationParameters.turnRate = [1,1] self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters) self.lastkey = key if (key==ord('T') and key is not self.lastkey): if self.doTransition: print('Disabling transition') self.doTransition = False else: print('Enabling transition') self.doTransition = True self.lastkey = key if self.doTransition: gpsPos = self.gps.getValues() if gpsPos[0] < self.waterPosx+2 and gpsPos[0] > self.waterPosx -0.5: gain = 4/2.5*(gpsPos[0]+0.5) + 1 self.SimulationParameters.drive = gain #print('Transitioning') self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters) self.NetworkParameters.set_frequencies(self.SimulationParameters) # Log data self.log_iteration()
def __init__(self, robot, n_iterations, parameters, logs="logs/log.npz"): super(SalamanderCMC, self).__init__() self.robot = robot timestep = int(robot.getBasicTimeStep()) self.network = SalamanderNetwork(1e-3*timestep, parameters) # Position sensors self.position_sensors = [ self.robot.getPositionSensor('position_sensor_{}'.format(i+1)) for i in range(self.N_BODY_JOINTS) ] + [ self.robot.getPositionSensor('position_sensor_leg_{}'.format(i+1)) for i in range(self.N_LEGS) ] for sensor in self.position_sensors: sensor.enable(timestep) # GPS self.gps = robot.getGPS("fgirdle_gps") self.gps.enable(timestep) # Get motors self.motors_body = [ self.robot.getMotor("motor_{}".format(i+1)) for i in range(self.N_BODY_JOINTS) ] self.motors_legs = [ self.robot.getMotor("motor_leg_{}".format(i+1)) for i in range(self.N_LEGS) ] # Set motors for motor in self.motors_body: motor.setPosition(0) motor.enableForceFeedback(timestep) motor.enableTorqueFeedback(timestep) for motor in self.motors_legs: motor.setPosition(-np.pi/2) motor.enableForceFeedback(timestep) motor.enableTorqueFeedback(timestep) # Iteration counter self.iteration = 0 # Logging self.log = ExperimentLogger( n_iterations, n_links=1, n_joints=self.N_BODY_JOINTS+self.N_LEGS, filename=logs, timestep=1e-3*timestep, **parameters ) #GPS stuff self.waterPosx = 0 self.NetworkParameters = self.network.parameters self.SimulationParameters = parameters self.doTransition = False self.keyboard = Keyboard() self.keyboard.enable(samplingPeriod=100) self.lastkey = 0
for m in range(4): motors[m].setPosition(math.inf) motors[m].setVelocity(1) print("Motors set!\n") imu = robot.getDevice('inertial unit') imu.enable(timestep) gps = robot.getDevice('gps') gps.enable(timestep) gyro = robot.getDevice('gyro') gyro.enable(timestep) kb = Keyboard() try: kb.enable(timestep) except: print("keyboard fail") print("Instruments and control initiated!\n") while robot.step(timestep) != -1: if robot.getTime() > 1.0: break # Display manual control message. print("You can control the drone with your computer keyboard:\n") print("- 'up': move forward.\n") print("- 'down': move backward.\n")
"""Simple robot controller.""" from controller import Robot, Keyboard, Pen, Display import sys, time, math from rovecomm import RoveComm, RoveCommPacket import socket, cv2, pickle, struct import numpy as np from rover import Rover rovecomm_node = RoveComm(11001, ("", 11112)) # Get pointer to the robot. robot = Robot() keyboard = Keyboard() keyboard.enable(64) # Initialize the rover class, with our rovecomm node rover = Rover(robot, rovecomm_node) rovecomm_node.set_callback(1000, rover.drive_callback) # Get simulation step length. timeStep = int(robot.getBasicTimeStep()) while robot.step(timeStep) != -1: ## Check if we need to trigger watchdog and stop driving rover.drive_watchdog_check() # send the sensor data to the autonomy program rover.send_sensor_update() # Stream the current frame (at fixed FPS)
class Mouse(Supervisor): """Main class for Mouse control. """ def __init__(self): super(Mouse, self).__init__() self.biomech = MusculoSkeletalSystem.MusculoSkeletalSystem( os.path.join('musculoskeletal', 'mouse.json') ) self.TIMESTEP = int(self.getBasicTimeStep()) self.motors = {} self.position_sensors = {} self.ground_contact_sensors = {} self.joint_positions = {} self.muscle_forces = {} self.ground_contacts = {} # Initialize webots self.key_press = None self.initialize_webots_motors() self.initialize_webots_position_sensors() self.initialize_webots_ground_contact_sensors() self.initialize_webots_keyboard() # Muscle visualization self.gps = {} self.mv_transform = {} self.mv_color = {} self.mv_geom = {} self.muscle_viz = None self.initialize_gps() self.initialize_muscle_visualizations() # Foot Trajectories [time, axis] self.ankle_r_trajectory = np.zeros([BUFFER_SIZE_TRAJECTORIES, 3]) self.ankle_l_trajectory = np.zeros([BUFFER_SIZE_TRAJECTORIES, 3]) self.iteration = 0 def __del__(self): """ Deletion """ try: os.stat(RESULTS_DIRECTORY) except: os.mkdir(RESULTS_DIRECTORY) np.save( RESULTS_DIRECTORY + "ankle_l_trajectory.npy", self.ankle_l_trajectory[:self.iteration, :] ) np.save( RESULTS_DIRECTORY + "ankle_r_trajectory.npy", self.ankle_r_trajectory[:self.iteration, :] ) return def run(self): """ Run """ reflex = Reflexes(self.biomech.sim_muscle_names) while self.step(self.TIMESTEP) != -1: # Reflex model reflex.step( self.update_joint_positions(), self.update_muscle_forces(), self.update_ground_contacts() ) activations = reflex.activations # Update the biomechanical model self.biomech.update( self.TIMESTEP / 1000., self.update_joint_positions(), activations ) # Get the Torque torque = self.biomech.joint_torque() for name, motor in self.motors.iteritems(): motor.setTorque(torque[name]) if MUSCLE_VISUALIZATION: self.muscle_viz.step(activations, viz=True) else: self.muscle_viz.step(activations, viz=False) # Save data self.iteration += 1 self.ankle_l_trajectory[self.iteration, :] = ( self.gps["LH_G1_ANKLE"].getValues() ) self.ankle_r_trajectory[self.iteration, :] = ( self.gps["RH_G1_ANKLE"].getValues() ) def initialize_webots_keyboard(self): """ Initialize webots keyboard """ self.key_press = Keyboard() self.key_press.enable(self.TIMESTEP * 25) def initialize_webots_motors(self): """Set-up leg joints in the system.""" for joint in self.biomech.sim_joints: print('Initializing webots motor : {}'.format(joint)) self.motors[joint] = self.getMotor(str(joint)) def initialize_webots_position_sensors(self): """Set-up leg joints in the system.""" for joint in self.biomech.sim_joints: print('Initializing webots motor : {}'.format(joint)) self.position_sensors[joint] = self.getPositionSensor( str(joint) + '_POS' ) self.position_sensors[joint].enable(self.TIMESTEP) def initialize_webots_ground_contact_sensors(self): """Initialize groung contact sensors.""" print('Initializing webots ground contact sensors ') self.ground_contact_sensors['LEFT_TOE_TOUCH'] = self.getTouchSensor( 'LEFT_TOE_TOUCH' ) self.ground_contact_sensors['LEFT_TOE_TOUCH'].enable(self.TIMESTEP) self.ground_contact_sensors['RIGHT_TOE_TOUCH'] = self.getTouchSensor( 'RIGHT_TOE_TOUCH' ) self.ground_contact_sensors['RIGHT_TOE_TOUCH'].enable(self.TIMESTEP) # MUSCLE VISUALIZATION def initialize_muscle_visualizations(self): """Initialize necessary attributes for muscle visualization.""" # Get muscle transform, appearance and geom for muscle in self.biomech.sim_muscle_names: muscle_split = muscle.split('_') side = muscle_split[0] name = muscle_split[-1] # TRANSFORM transform = str(side + '_MV_TRANSFORM_' + name) self.mv_transform[transform] = self.getFromDef(transform) # COLOR appearance = str(side + '_MV_COLOR_' + name) self.mv_color[appearance] = self.getFromDef(appearance) # GEOM geom = str(side + '_MV_GEOM_' + name) self.mv_geom[geom] = self.getFromDef(geom) # Creat muscle muscle visualization object self.muscle_viz = MuscleVisualization( self.muscle_visualization_attachment(), self.gps, self.mv_transform, self.mv_color, self.mv_geom) return def initialize_gps(self): """Initialize gps nodes for muscle visualization.""" GPS_NAMES = ['G1_PELVIS', 'G2_PELVIS', 'G1_HIP', 'G2_HIP', 'G1_KNEE', 'G2_KNEE', 'G1_ANKLE'] sides = ['LH', 'RH'] for side in sides: for gps in GPS_NAMES: name = side + '_' + gps self.gps[name] = self.getGPS(name) self.gps[name].enable(self.TIMESTEP) return def muscle_visualization_attachment(self): """Returns the dictionaries muscle origin and insertion with respect to GPS.""" muscle_attach = { 'PMA': ['G1_PELVIS', 'G1_HIP'], 'CF': ['G2_PELVIS', 'G1_HIP'], 'SM': ['G2_PELVIS', 'G1_KNEE'], 'POP': ['G1_HIP', 'G1_KNEE'], 'RF': ['G1_HIP', 'G2_HIP'], 'TA': ['G1_KNEE', 'G1_ANKLE'], 'SOL': ['G1_KNEE', 'G2_KNEE'], 'LG': ['G1_HIP', 'G2_KNEE'] } return muscle_attach def update_joint_positions(self): """ Initialize the array to store joint positions.""" for name, sensor in self.position_sensors.iteritems(): self.joint_positions[name] = ( sensor.getValue() + self.biomech.sim_joints[name].reference_angle ) return self.joint_positions def update_muscle_forces(self): """ Initialize the array to store joint positions.""" for muscle in self.biomech.sim_muscles: self.muscle_forces[muscle] = self.biomech.sim_muscles[ muscle ].tendonForce return self.muscle_forces def update_ground_contacts(self): """ Update ground contacts """ self.ground_contacts['L'] = self.ground_contact_sensors[ 'LEFT_TOE_TOUCH' ].getValue() self.ground_contacts['R'] = self.ground_contact_sensors[ 'RIGHT_TOE_TOUCH' ].getValue() return self.ground_contacts
def default_high_pos(): for i in range(6): motor_lst[0 + i*3].setPosition(0) motor_lst[0 + i*3].setVelocity(1.0) motor_lst[2 + i*3].setPosition(math.pi * -1 / 8) motor_lst[2 + i*3].setVelocity(1.0) motor_lst[1 + i*3].setPosition(math.pi * -3 / 8) motor_lst[1 + i*3].setVelocity(1.0) default_high_pos() keyboard = Keyboard() keyboard.enable(60) while robot.step(timestep) != -1: #motor_lst[0].setPosition(10.0) #motor_lst[0].setVelocity(1.0) key=keyboard.getKey() if (key==ord('D')): print('D is pressed') rotate(math.pi * -2 / 8) elif (key==ord('F')): print('F is pressed') rotate(math.pi * 2 / 8)
class SupervisorController: def __init__(self, ros_active=False, mode='normal', do_ros_init=True, base_ns='', model_states_active=True): """ The SupervisorController, a Webots controller that can control the world. Set the environment variable WEBOTS_ROBOT_NAME to "supervisor_robot" if used with 1_bot.wbt or 4_bots.wbt. :param ros_active: Whether to publish ROS messages :param mode: Webots mode, one of 'normal', 'paused', or 'fast' :param do_ros_init: Whether rospy.init_node should be called :param base_ns: The namespace of this node, can normally be left empty """ # requires WEBOTS_ROBOT_NAME to be set to "supervisor_robot" self.ros_active = ros_active self.model_states_active = model_states_active self.time = 0 self.clock_msg = Clock() self.supervisor = Supervisor() self.keyboard_activated = False if mode == 'normal': self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) elif mode == 'paused': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE) elif mode == 'fast': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST) else: self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) self.robot_nodes = {} self.translation_fields = {} self.rotation_fields = {} self.joint_nodes = {} self.link_nodes = {} root = self.supervisor.getRoot() children_field = root.getField('children') children_count = children_field.getCount() for i in range(children_count): node = children_field.getMFNode(i) name_field = node.getField('name') if name_field is not None and node.getType() == Node.ROBOT: # this is a robot name = name_field.getSFString() if name == "supervisor_robot": continue self.robot_nodes[name] = node self.translation_fields[name] = node.getField("translation") self.rotation_fields[name] = node.getField("rotation") self.joint_nodes[name], self.link_nodes[ name] = self.collect_joint_and_link_node_references( node, {}, {}) if self.ros_active: # need to handle these topics differently or we will end up having a double // if base_ns == "": clock_topic = "/clock" model_topic = "/model_states" else: clock_topic = base_ns + "clock" model_topic = base_ns + "model_states" if do_ros_init: rospy.init_node("webots_ros_supervisor", argv=['clock:=' + clock_topic]) self.clock_publisher = rospy.Publisher(clock_topic, Clock, queue_size=1) self.model_state_publisher = rospy.Publisher(model_topic, ModelStates, queue_size=1) self.reset_service = rospy.Service(base_ns + "reset", Empty, self.reset) self.reset_pose_service = rospy.Service(base_ns + "reset_pose", Empty, self.set_initial_poses) self.set_robot_pose_service = rospy.Service( base_ns + "set_robot_pose", SetObjectPose, self.robot_pose_callback) self.reset_ball_service = rospy.Service(base_ns + "reset_ball", Empty, self.reset_ball) self.set_ball_position_service = rospy.Service( base_ns + "set_ball_position", SetObjectPosition, self.ball_pos_callback) self.world_info = self.supervisor.getFromDef("world_info") self.ball = self.supervisor.getFromDef("ball") def collect_joint_and_link_node_references(self, node, joint_dict, link_dict): # this is a recursive function that iterates through the whole robot as this seems to be the only way to # get all joints # add node if it is a joint if node.getType() == Node.SOLID: name = node.getDef() link_dict[name] = node if node.getType() == Node.HINGE_JOINT: name = node.getDef() # substract the "Joint" keyword due to naming convention name = name[:-5] joint_dict[name] = node # the joints dont have children but an "endpoint" that we need to search through if node.isProto(): endpoint_field = node.getProtoField('endPoint') else: endpoint_field = node.getField('endPoint') endpoint_node = endpoint_field.getSFNode() self.collect_joint_and_link_node_references( endpoint_node, joint_dict, link_dict) # needs to be done because Webots has two different getField functions for proto nodes and normal nodes if node.isProto(): children_field = node.getProtoField('children') else: children_field = node.getField('children') if children_field is not None: for i in range(children_field.getCount()): child = children_field.getMFNode(i) self.collect_joint_and_link_node_references( child, joint_dict, link_dict) return joint_dict, link_dict def step_sim(self): self.time += self.timestep / 1000 self.supervisor.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_clock() if self.model_states_active: self.publish_model_states() def handle_gui(self): if not self.keyboard_activated: self.keyboard = Keyboard() self.keyboard.enable(100) self.keyboard_activated = True key = self.keyboard.getKey() if key == ord('R'): self.reset() elif key == ord('P'): self.set_initial_poses() elif key == Keyboard.SHIFT + ord('R'): try: self.reset_ball() except AttributeError: print("No ball in simulation that can be reset") return key def publish_clock(self): self.clock_msg.clock = rospy.Time.from_seconds(self.time) self.clock_publisher.publish(self.clock_msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFFloat(9.81) else: self.world_info.getField("gravity").setSFFloat(0) def set_self_collision(self, active, name="amy"): self.robot_nodes[name].getField("selfCollision").setSFBool(active) def reset_robot_pose(self, pos, quat, name="amy"): self.set_robot_pose_quat(pos, quat, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_pose_rpy(pos, rpy, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset(self, req=None): self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() return EmptyResponse() def reset_robot_init(self, name="amy"): self.robot_nodes[name].loadState('__init__') self.robot_nodes[name].resetPhysics() def set_initial_poses(self, req=None): self.reset_robot_pose_rpy([-1, 3, 0.42], [0, 0.24, -1.57], name="amy") self.reset_robot_pose_rpy([-1, -3, 0.42], [0, 0.24, 1.57], name="rory") self.reset_robot_pose_rpy([-3, 3, 0.42], [0, 0.24, -1.57], name="jack") self.reset_robot_pose_rpy([-3, -3, 0.42], [0, 0.24, 1.57], name="donna") self.reset_robot_pose_rpy([0, 6, 0.42], [0, 0.24, -1.57], name="melody") return EmptyResponse() def robot_pose_callback(self, req=None): self.reset_robot_pose( [req.pose.position.x, req.pose.position.y, req.pose.position.z], [ req.pose.orientation.x, req.pose.orientation.y, req.pose.orientation.z, req.pose.orientation.w ], req.object_name) return SetObjectPoseResponse() def reset_ball(self, req=None): self.ball.getField("translation").setSFVec3f([0, 0, 0.0772]) self.ball.getField("rotation").setSFRotation([0, 0, 1, 0]) self.ball.resetPhysics() return EmptyResponse() def ball_pos_callback(self, req=None): self.set_ball_pose([req.position.x, req.position.y, req.position.z]) return SetObjectPositionResponse() def set_ball_pose(self, pos): self.ball.getField("translation").setSFVec3f(list(pos)) self.ball.resetPhysics() def get_ball_pose(self): return self.ball.getField("translation").getSFVec3f() def get_ball_velocity(self): if self.ball: return self.ball.getVelocity() def node(self): s = self.supervisor.getSelected() if s is not None: print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}") def set_robot_axis_angle(self, axis, angle, name="amy"): if name in self.rotation_fields: self.rotation_fields[name].setSFRotation( list(np.append(axis, angle))) def set_robot_rpy(self, rpy, name="amy"): axis, angle = transforms3d.euler.euler2axangle(rpy[0], rpy[1], rpy[2], axes='sxyz') self.set_robot_axis_angle(axis, angle, name) def set_robot_quat(self, quat, name="amy"): axis, angle = transforms3d.quaternions.quat2axangle( [quat[3], quat[0], quat[1], quat[2]]) self.set_robot_axis_angle(axis, angle, name) def set_robot_position(self, pos, name="amy"): if name in self.translation_fields: self.translation_fields[name].setSFVec3f(list(pos)) def set_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_position(pos, name) self.set_robot_rpy(rpy, name) def set_robot_pose_quat(self, pos, quat, name="amy"): self.set_robot_position(pos, name) self.set_robot_quat(quat, name) def get_robot_position(self, name="amy"): if name in self.translation_fields: return self.translation_fields[name].getSFVec3f() def get_robot_orientation_axangles(self, name="amy"): if name in self.rotation_fields: return self.rotation_fields[name].getSFRotation() def get_robot_orientation_rpy(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) return list( transforms3d.euler.axangle2euler(ax_angle[:3], ax_angle[3], axes='sxyz')) def get_robot_orientation_quat(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) # transforms 3d uses scalar (i.e. the w part in the quaternion) first notation of quaternions, ros uses scalar last quat_scalar_first = transforms3d.quaternions.axangle2quat( ax_angle[:3], ax_angle[3]) quat_scalar_last = np.append(quat_scalar_first[1:], quat_scalar_first[0]) return list(quat_scalar_last) def get_robot_pose_rpy(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_rpy( name) def get_robot_pose_quat(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_quat( name) def get_robot_velocity(self, name="amy"): velocity = self.robot_nodes[name].getVelocity() return velocity[:3], velocity[3:] def get_link_pose(self, link, name="amy"): link_node = self.robot_nodes[name].getFromProtoDef(link) if not link_node: return None link_position = link_node.getPosition() mat = link_node.getOrientation() link_orientation = transforms3d.quaternions.mat2quat(np.array(mat)) return link_position, np.append(link_orientation[1:], link_orientation[0]) def get_link_velocities(self, link, name="amy"): """Returns linear and angular velocities""" link_node = self.robot_nodes[name].getFromProtoDef(link) velocity = link_node.getVelocity() return velocity[:3], velocity[3:] def publish_model_states(self): if self.model_state_publisher.get_num_connections() != 0: msg = ModelStates() for robot_name, robot_node in self.robot_nodes.items(): position, orientation = self.get_robot_pose_quat( name=robot_name) robot_pose = Pose() robot_pose.position = Point(*position) robot_pose.orientation = Quaternion(*orientation) msg.name.append(robot_name) msg.pose.append(robot_pose) lin_vel, ang_vel = self.get_robot_velocity(robot_name) twist = Twist() twist.linear.x = lin_vel[0] twist.linear.y = lin_vel[1] twist.linear.z = lin_vel[2] twist.angular.x = ang_vel[0] twist.angular.y = ang_vel[1] twist.angular.z = ang_vel[2] msg.twist.append(twist) head_node = robot_node.getFromProtoDef("head") head_position = head_node.getPosition() head_orientation = head_node.getOrientation() head_orientation_quat = transforms3d.quaternions.mat2quat( np.reshape(head_orientation, (3, 3))) head_pose = Pose() head_pose.position = Point(*head_position) head_pose.orientation = Quaternion(head_orientation_quat[1], head_orientation_quat[2], head_orientation_quat[3], head_orientation_quat[0]) msg.name.append(robot_name + "_head") msg.pose.append(head_pose) if self.ball is not None: ball_position = self.ball.getField("translation").getSFVec3f() ball_pose = Pose() ball_pose.position = Point(*ball_position) ball_pose.orientation = Quaternion() msg.name.append("ball") msg.pose.append(ball_pose) self.model_state_publisher.publish(msg)
# KEYBOARD CONTROLLED VEHICLE """ In this simulation, The vehicle gets input from the user's keyboard command and acts accordingly.The purpose of this simulation is to get used to the Webots environment. """ # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, Keyboard from controller import Robot, Motor, Keyboard import time TIME_STEP = 64 #timestep should be the same as in the WorldInfo # create the Robot and Keyboard instance. robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) # assign the motors to wheels. wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(robot.getMotor(wheelsNames[i])) wheels[i].setPosition( float('inf')) #set position infinity not to restrict motion wheels[i].setVelocity(0.0) #set initial speed as 0 leftSpeed = 0.0 rightSpeed = 0.0 print('Click the simulation window before the keyboard command!') print('Press [W] or [w] to move forward')
def __init__(self, controller): self.controller = controller self.keyboard = Keyboard() self.keyboard.enable(self.controller.get_timestep())
right_motor = robot.getMotor("right motor") left_motor_sensor = left_motor.getPositionSensor() right_motor_sensor = right_motor.getPositionSensor() left_motor_sensor.enable(timestep) right_motor_sensor.enable(timestep) left_motor_init = left_motor_sensor.getValue() right_motor_init = right_motor_sensor.getValue() left_motor.setPosition(float('inf')) right_motor.setPosition(float('inf')) #KEYBOARD INITIALIZATION keyboard = Keyboard() keyboard.enable(timestep) print("Select the 3D window and control the robot using the W, A, S, D keys.") #LIDAR INITIALIZATION lidar = robot.getLidar("lidar") lidar.enable(timestep) lidar.enablePointCloud() #FRONT CAMERA INITIALIZATION front_camera = robot.getCamera("front_camera") front_camera.enable(timestep) #REAR CAMERA INITIALIZATION rear_camera = robot.getCamera("rear_camera") rear_camera.enable(timestep) while robot.step(timestep) != -1:
"""uav_control controller.""" # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot, Camera, InertialUnit, GPS, Compass, Gyro, Motor, Keyboard, Emitter import math import numpy as np import struct # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) keyboard = Keyboard() keyboard.enable(timestep) emitter = robot.getEmitter("emitter") # send data message = struct.pack('5f', 0.0, 0.0, 0.0, 0.0, 0.0) emitter.send(message) imu = robot.getInertialUnit("inertial unit") imu.enable(timestep) camera = robot.getCamera("camera") camera.enable(timestep) gps = robot.getGPS("gps") gps.enable(timestep) compass = robot.getCompass("compass") compass.enable(timestep) gyro = robot.getGyro("gyro") gyro.enable(timestep) camera_roll_motor = robot.getCamera("camera roll") camera_pitch_motor = robot.getCamera("camera pitch")