def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self, state) # Sensor readings in world units self.parameters.sensor_distances = self.get_ir_distances() self.parameters.pose = self.pose_est
def process_state_info(self,state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self,state) # Sensor readings in world units self.parameters.sensor_distances = self.get_ir_distances() self.parameters.pose = self.pose_est
def __init__(self, robot_pose, robot_info, options = None): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) self.extgoal = False if options is not None: try: self.parameters.goal.x = options.x self.parameters.goal.y = options.y self.extgoal = True except Exception: pass # Fill in some parameters self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.parameters.ir_max = robot_info.ir_sensors.rmax self.parameters.direction = 'left' self.parameters.distance = 0.2 self.robot = robot_info # generate global path self.parameters.ga_path = ga.ga_execute((0,0), (self.parameters.goal.x, self.parameters.goal.y)) self.parameters.ga_path.append((self.parameters.goal.x, self.parameters.goal.y)) global global_cnt global_cnt = len(self.parameters.ga_path) point_cnt = self.parameters.point_cnt #print self.parameters.ga_path, "ga_path" #Add controllers self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.gtg = self.create_controller('GoToGoal', self.parameters) self.hold = self.create_controller('Hold', None) self.wall = self.create_controller('FollowWall', self.parameters) self.path = self.create_controller('FollowPath', self.parameters) self.blending = self.create_controller("Blending", self.parameters) # Define transitions self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold), (self.at_obstacle, self.avoidobstacles)) self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.path) ) # self.add_controller(self.blending, # (self.at_goal, self.hold), # (self.free, self.path), # (self.at_obstacle, self.avoidobstacles), # ) self.add_controller(self.path, (lambda: self.next_point(), self.path), (self.at_goal, self.hold), #(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg), (self.at_obstacle, self.avoidobstacles)) self.current = self.path
def __init__(self, robot_pose, robot_info, options=None): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) self.extgoal = False if options is not None: try: self.parameters.goal.x = options.x self.parameters.goal.y = options.y self.extgoal = True except Exception: pass # Fill in some parameters self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.parameters.ir_max = robot_info.ir_sensors.rmax self.parameters.direction = 'left' self.parameters.distance = 0.2 self.robot = robot_info # generate global path self.parameters.ga_path = ga.ga_execute( (0, 0), (self.parameters.goal.x, self.parameters.goal.y)) self.parameters.ga_path.append( (self.parameters.goal.x, self.parameters.goal.y)) global global_cnt global_cnt = len(self.parameters.ga_path) point_cnt = self.parameters.point_cnt #print self.parameters.ga_path, "ga_path" #Add controllers self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.gtg = self.create_controller('GoToGoal', self.parameters) self.hold = self.create_controller('Hold', None) self.wall = self.create_controller('FollowWall', self.parameters) self.path = self.create_controller('FollowPath', self.parameters) self.blending = self.create_controller("Blending", self.parameters) # Define transitions self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold), (self.at_obstacle, self.avoidobstacles)) self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.path)) # self.add_controller(self.blending, # (self.at_goal, self.hold), # (self.free, self.path), # (self.at_obstacle, self.avoidobstacles), # ) self.add_controller( self.path, (lambda: self.next_point(), self.path), (self.at_goal, self.hold), #(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg), (self.at_obstacle, self.avoidobstacles)) self.current = self.path
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self, renderer) # Make sure to have all headings: renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 # Ensure the headings are calculated # Draw arrow to goal if self.current == self.gtg: goal_angle = self.gtg.get_heading_angle(self.parameters) renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle)) # Draw arrow for PathFollowing elif self.current == self.path: goal_angle = self.path.get_heading_angle(self.parameters) renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle)) # elif self.current == self.blending: # goal_angle = self.blending.get_heading_angle(self.parameters) # renderer.set_pen(0x0000FF) # renderer.draw_arrow(0,0, # arrow_length*cos(goal_angle), # arrow_length*sin(goal_angle)) # Draw arrow away from obstacles elif self.current == self.avoidobstacles: away_angle = self.avoidobstacles.get_heading_angle(self.parameters) renderer.set_pen(0xCC3311) renderer.draw_arrow(0, 0, arrow_length * cos(away_angle), arrow_length * sin(away_angle)) elif self.current == self.wall: # Draw vector to wall: renderer.set_pen(0x0000FF) renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) # Draw renderer.set_pen(0xFF00FF) renderer.push_state() renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0], self.wall.along_wall_vector[1]) renderer.pop_state() # Draw heading (who knows, it might not be along_wall) renderer.set_pen(0xFF00FF) renderer.draw_arrow(0, 0, arrow_length * cos(self.wall.heading_angle), arrow_length * sin(self.wall.heading_angle))
def __init__(self, robot_pose, robot_info): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) self.jcontroller = self.create_controller('joystick.JoystickController', \ (self.parameters.joystick, robot_info.wheels.max_velocity*robot_info.wheels.radius, robot_info.wheels.max_velocity*robot_info.wheels.radius/robot_info.wheels.base_length)) self.current = self.jcontroller
def set_parameters(self,params): """Set parameters for itself and the controllers""" QuickBotSupervisor.set_parameters(self,params) self.gtg.set_parameters(self.parameters) self.avoidobstacles.set_parameters(self.parameters) self.wall.set_parameters(self.parameters) #self.pp.set_parameters(self.parameters) self.path.set_parameters(self.parameters) self.blending.set_parameters(self.parameters)
def set_parameters(self, params): """Set parameters for itself and the controllers""" QuickBotSupervisor.set_parameters(self, params) self.gtg.set_parameters(self.parameters) self.avoidobstacles.set_parameters(self.parameters) self.wall.set_parameters(self.parameters) #self.pp.set_parameters(self.parameters) self.path.set_parameters(self.parameters) self.blending.set_parameters(self.parameters)
def __init__(self, robot_pose, robot_info, options = None): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) self.extgoal = False if options is not None: try: self.parameters.goal.x = options.x self.parameters.goal.y = options.y self.extgoal = True except Exception: pass # Fill in some parameters self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.parameters.ir_max = robot_info.ir_sensors.rmax self.parameters.direction = 'left' self.parameters.distance = 0.2 self.robot = robot_info #Add controllers self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.gtg = self.create_controller('GoToGoal', self.parameters) self.wall = self.create_controller('FollowWall', self.parameters) self.hold = self.create_controller('Hold', None) self.gs = self.create_controller('GoStraight', None) # Week 7 Assignment: # Define transitions self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold), (self.at_obstacle, self.avoidobstacles)) self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.gtg), ) # Change and add additional transitions self.add_controller(self.gs, (self.at_obstacle, self.wall) ) self.add_controller(self.wall, (self.at_goal, self.hold) ) # End Week 7 # Start in the 'go-to-goal' state #self.current = self.gtg self.current = self.gs
def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self,state) # The pose for controllers self.parameters.pose = self.pose_est # Update the trajectory self.tracker.add_point(self.pose_est)
def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self, state) # The pose for controllers self.parameters.pose = self.pose_est # Update the trajectory self.tracker.add_point(self.pose_est)
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self, renderer) renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y)) arrow_length = self.robot_size * 5 # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(self.gtg.heading_angle), arrow_length * sin(self.gtg.heading_angle))
def __init__(self, robot_pose, robot_info): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Create the controller self.blending = self.create_controller('week5.Blending', self.parameters) # Set the controller self.current = self.blending
def __init__(self, robot_pose, robot_info): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Create the controller self.avoidobstacles = self.create_controller('week4.AvoidObstacles', self.parameters) # Set the controller self.current = self.avoidobstacles
def __init__(self, robot_pose, robot_info): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Create the tracker self.tracker = Path(robot_pose, 0) # Create the controller self.gtg = self.create_controller('week3.GoToGoal', self.parameters) # Set the controller self.current = self.gtg
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self,renderer) renderer.set_pose(Pose(self.pose_est.x,self.pose_est.y)) arrow_length = self.robot_size*5 # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(self.gtg.heading_angle), arrow_length*sin(self.gtg.heading_angle))
def draw(self, renderer): """Draw controller info""" QuickBotSupervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle))
def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self,state) # The pose for controllers self.parameters.pose = self.pose_est # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances() # Distance to the goal self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) return self.parameters
def draw(self, renderer): """Draw controller info""" QuickBotSupervisor.draw(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle))
def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self, state) # The pose for controllers self.parameters.pose = self.pose_est # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances() # Distance to the goal self.distance_from_goal = sqrt( (self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) return self.parameters
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self,renderer) # Make sure to have all headings: renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 # Ensure the headings are calculated # Draw arrow to goal if self.current == self.gtg: goal_angle = self.gtg.get_heading_angle(self.parameters) renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle)) # Draw arrow away from obstacles elif self.current == self.avoidobstacles: away_angle = self.avoidobstacles.get_heading_angle(self.parameters) renderer.set_pen(0xCC3311) renderer.draw_arrow(0,0, arrow_length*cos(away_angle), arrow_length*sin(away_angle)) elif self.current == self.wall: # Draw vector to wall: renderer.set_pen(0x0000FF) renderer.draw_arrow(0,0, self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) # Draw renderer.set_pen(0xFF00FF) renderer.push_state() renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) renderer.draw_arrow(0,0, self.wall.along_wall_vector[0], self.wall.along_wall_vector[1]) renderer.pop_state() # Draw heading (who knows, it might not be along_wall) renderer.set_pen(0xFF00FF) renderer.draw_arrow(0,0, arrow_length*cos(self.wall.heading_angle), arrow_length*sin(self.wall.heading_angle))
def __init__(self, robot_pose, robot_info, options=None): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) self.extgoal = False if options is not None: try: self.parameters.goal.x = options.x self.parameters.goal.y = options.y self.extgoal = True except Exception: pass # Fill in some parameters self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.parameters.ir_max = robot_info.ir_sensors.rmax self.parameters.direction = 'left' self.parameters.distance = 0.2 self.robot = robot_info #Add controllers self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.gtg = self.create_controller('GoToGoal', self.parameters) self.wall = self.create_controller('FollowWall', self.parameters) self.hold = self.create_controller('Hold', None) # Week 7 Assignment: # Define transitions self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold), (self.at_obstacle, self.avoidobstacles)) self.add_controller( self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.gtg), ) # Change and add additional transitions # End Week 7 # Start in the 'go-to-goal' state self.current = self.gtg
def __init__(self, robot_pose, robot_info, options = None): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) if options is not None: try: self.parameters.direction = options.direction self.parameters.velocity.v = options.velocity self.parameters.gains = options.gains except: pass # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Create and set the controller self.current = self.create_controller('week6.FollowWall', self.parameters)
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal/obstacle self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold)) # Start in the 'go-to-goal' state self.current = self.gtg
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal/obstacle self.add_controller(self.hold, (lambda : not self.at_goal(), self.gtg)) self.add_controller(self.gtg, (self.at_goal, self.hold)) # Start in the 'go-to-goal' state self.current = self.gtg
def __init__(self, robot_pose, robot_info, options=None): """Create the controller""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) if options is not None: try: self.parameters.direction = options.direction self.parameters.velocity.v = options.velocity self.parameters.gains = options.gains except: pass # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Create and set the controller self.current = self.create_controller('week6.FollowWall', self.parameters)
def process_state_info(self, state): """Update state parameters for the controllers and self""" QuickBotSupervisor.process_state_info(self,state) # The pose for controllers self.parameters.pose = self.pose_est # Distance to the goal # Added for swarm robots goal = goal_location[(self.robot.robot_id + 1)%4] self.distance_from_goal = sqrt((self.pose_est.x - goal.x)**2 + (self.pose_est.y - goal.y)**2) #self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances() # Distance to the closest obstacle self.distmin = min(self.parameters.sensor_distances)
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 # Ensure the headings are calculated away_angle = self.avoidobstacles.get_heading_angle(self.parameters) goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal if self.current == self.gtg: renderer.set_pen(0x00FF00,0.01) else: renderer.set_pen(0xA000FF00) renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle)) # Draw arrow away from obstacles if self.current == self.avoidobstacles: renderer.set_pen(0xFF0000,0.01) else: renderer.set_pen(0xA0FF0000) renderer.draw_arrow(0,0, arrow_length*cos(away_angle), arrow_length*sin(away_angle)) if "blending" in self.__dict__: blend_angle = self.blending.get_heading_angle(self.parameters) # Draw the blending if self.current == self.blending: renderer.set_pen(0xFF, 0.01) else: renderer.set_pen(0xA00000FF) renderer.draw_arrow(0,0, arrow_length*cos(blend_angle), arrow_length*sin(blend_angle))
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 # Draw arrow to goal renderer.set_pen(0xCC00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.goal_angle), arrow_length * sin(self.blending.goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xCCFF0000) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.away_angle), arrow_length * sin(self.blending.away_angle)) # Draw heading renderer.set_pen(0xFF, 0.02) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.heading_angle), arrow_length * sin(self.blending.heading_angle))
def __init__(self, robot_pose, robot_info): """Create necessary controllers""" QuickBotSupervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Create the controllers self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.gtg = self.create_controller('GoToGoal', self.parameters) self.hold = self.create_controller('Hold', None) # Create some state transitions self.add_controller(self.hold) self.add_controller(self.gtg, \ (self.at_goal, self.hold), \ (self.at_obstacle, self.avoidobstacles)) # Week 5 Assigment code should go here # End Week 5 Assignment # Start in 'go-to-goal' state self.current = self.gtg
def draw_foreground(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_foreground(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 # Draw arrow to goal renderer.set_pen(0xCC00FF00) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.goal_angle), arrow_length*sin(self.blending.goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xCCFF0000) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.away_angle), arrow_length*sin(self.blending.away_angle)) # Draw heading renderer.set_pen(0xFF,0.02) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.heading_angle), arrow_length*sin(self.blending.heading_angle))
def set_parameters(self,params): """Set parameters for itself and the controllers""" QuickBotSupervisor.set_parameters(self,params) self.gtg.set_parameters(self.parameters)
def draw_background(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_background(self, renderer) # Draw robot path self.tracker.draw(renderer)
def draw_background(self, renderer): """Draw controller info""" QuickBotSupervisor.draw_background(self,renderer) # Draw robot path self.tracker.draw(renderer)
def set_parameters(self, params): """Set parameters for itself and the controllers""" QuickBotSupervisor.set_parameters(self, params) self.blending.set_parameters(self.parameters)