def test_planner_to_goal(): vec_env, custom_draws = make_block_push_env(two_d=True) start_state_str = vec_env.get_pillar_state()[0] start_state = State.create_from_serialized_string(start_state_str) goal_state = State.create_from_serialized_string(start_state_str) goal_pose = np.array(start_state.get_values_as_vec([block_pos_fqn])) goal_pose[0] = 0.2 goal_pose[2] = 0.04 goal_state.set_values_from_vec([block_pos_fqn], goal_pose.tolist()) planner = Planner() planner.plan(start_state.get_serialized_string(), goal_state.get_serialized_string())
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller()
def test_one_push_planner(): vec_env, custom_draws = make_block_push_env(two_d=True) start_state_str = vec_env.get_pillar_state()[0] start_state = State.create_from_serialized_string(start_state_str) goal_state = State.create_from_serialized_string(start_state_str) goal_pose = np.array(start_state.get_values_as_vec([block_pos_fqn])) goal_pose[0] -= 0.05 goal_state.set_values_from_vec([block_pos_fqn], goal_pose.tolist()) planner = Planner(vec_env.cfg) one_push_plan = planner.plan(start_state.get_serialized_string(), goal_state.get_serialized_string()) plans = [one_push_plan] agent = Agent("test_planner") agent.collect_transition_data(vec_env, plans)
def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ["yellow", "blue"] assert our_side in ["left", "right"] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration, ) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller()
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller() def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) attacker_actions = self.planner.plan('attacker') defender_actions = self.planner.plan('defender') if self.attacker is not None: self.attacker.execute(self.arduino, attacker_actions) if self.defender is not None: self.defender.execute(self.arduino, defender_actions) # Information about the grabbers from the world grabbers = { 'our_defender': self.planner._world.our_defender.catcher_area, 'our_attacker': self.planner._world.our_attacker.catcher_area } # Information about states attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state) defenderState = (self.planner.defender_state, self.planner.defender_strat_state) # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, attackerState, defenderState, attacker_actions, defender_actions, grabbers, our_color=self.color, our_side=self.side, key=c, preprocess=pre_options) counter += 1 except: if self.defender is not None: self.defender.shutdown(self.arduino) if self.attacker is not None: self.attacker.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.attacker is not None: self.attacker.shutdown(self.arduino) if self.defender is not None: self.defender.shutdown(self.arduino)
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600) time.sleep(0.5) self.arduino.grabberUp() self.arduino.grab() # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino) self.color = color self.side = our_side self.timeOfNextAction = 0 self.preprocessing = Preprocessing() #it doesn't matter whether it is an Attacker or a Defender Controller self.controller = Attacker_Controller(self.planner._world, self.GUI) self.robot_action_list = []
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600) time.sleep(0.5) self.arduino.grabberUp() self.arduino.grab() # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino) self.color = color self.side = our_side self.timeOfNextAction = 0 self.preprocessing = Preprocessing() #it doesn't matter whether it is an Attacker or a Defender Controller self.controller = Attacker_Controller(self.planner._world, self.GUI) self.robot_action_list = [] def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) if time.time() >= self.timeOfNextAction: if self.robot_action_list == []: plan = self.planner.plan() if isinstance(plan, list): self.robot_action_list = plan else: self.robot_action_list = [(plan, 0)] if self.controller is not None: self.controller.execute(self.arduino, self) # Information about the grabbers from the world grabbers = { 'our_defender': self.planner._world.our_defender.catcher_area, 'our_attacker': self.planner._world.our_attacker.catcher_area } # Information about states robotState = 'test' # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, robotState, "we dont need it", '', "we dont need it", grabbers, our_color='blue', our_side=self.side, key=c, preprocess=pre_options) counter += 1 if c == ord('a'): self.arduino.grabberUp() self.arduino.grab() except: if self.controller is not None: self.controller.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.controller is not None: self.controller.shutdown(self.arduino)
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ["yellow", "blue"] assert our_side in ["left", "right"] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration, ) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller() def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed["frame"] if "background_sub" in preprocessed: cv2.imshow("bg sub", preprocessed["background_sub"]) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) attacker_actions = self.planner.plan("attacker") defender_actions = self.planner.plan("defender") if self.attacker is not None: self.attacker.execute(self.arduino, attacker_actions) if self.defender is not None: self.defender.execute(self.arduino, defender_actions) # Information about the grabbers from the world grabbers = { "our_defender": self.planner._world.our_defender.catcher_area, "our_attacker": self.planner._world.our_attacker.catcher_area, } # Information about states attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state) defenderState = (self.planner.defender_state, self.planner.defender_strat_state) # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, attackerState, defenderState, attacker_actions, defender_actions, grabbers, our_color=self.color, our_side=self.side, key=c, preprocess=pre_options, ) counter += 1 except: if self.defender is not None: self.defender.shutdown(self.arduino) if self.attacker is not None: self.attacker.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.attacker is not None: self.attacker.shutdown(self.arduino) if self.defender is not None: self.defender.shutdown(self.arduino)