Пример #1
0
class QuickBotSupervisor(Supervisor):
    """The QuickBotSupervisor inherits from the superclass 'supervisor.Supervisor' to implement detailed calculations for any inheriting QuickBot supervisor. 

    Most importantly, the QuickBotSupervisor object implements the system functions necessary to operate a QuickBot, namely the uni2diff unicycle to differential motion model conversion, the Jacobian problem, and any other computationally complex interface.

    The UI may use the get_parameters function interface to create docker windows for real-time update of the PID parameters. This is an advanced implementation and is not required for students to properly implement their own supervisors."""
    def __init__(self, robot_pose, robot_info):
        """Initialize internal variables"""
        Supervisor.__init__(self, robot_pose, robot_info)

        # initialize memory registers
        self.prev_left_ticks = robot_info.wheels.left_ticks
        self.prev_right_ticks = robot_info.wheels.right_ticks

        # Create tracker
        self.tracker = Path(robot_pose, 0)

        # Create & set the controller
        self.current = self.create_controller('GoToAngle', self.parameters)

    def init_default_parameters(self):
        """Sets the default PID parameters, goal, and velocity"""
        p = Struct()
        p.goal = 45.0
        p.velocity = 0.2
        p.pgain = 3.0

        self.parameters = p

    def get_ui_description(self, p=None):
        """Returns the UI description for the docker"""
        if p is None:
            p = self.parameters

        return [(('goal', 'Target angle'), p.goal), ('velocity', p.velocity),
                (('pgain', "Proportional gain"), p.pgain)]

    def set_parameters(self, params):
        """Set parameters for itself and the controllers"""
        self.parameters.goal = params.goal
        self.parameters.velocity = params.velocity
        self.parameters.pgain = params.pgain
        self.current.set_parameters(self.parameters)

    def uni2diff(self, uni):
        """Convert from unicycle model to differential model"""
        (v, w) = uni

        #Insert Week 2 Assignment Code Here

        # R = self.robot.wheels.radius
        # L = self.robot.wheels.base_length

        vl = 0
        vr = 0

        #End Week 2 Assignment Code

        return (vl, vr)

    def estimate_pose(self):
        """Update self.pose_est using odometry"""

        #Insert Week 2 Assignment Code Here

        # Get tick updates
        #self.robot.wheels.left_ticks
        #self.robot.wheels.right_ticks

        # Save the wheel encoder ticks for the next estimate

        #Get the present pose estimate
        x, y, theta = self.pose_est

        #Use your math to update these variables...
        theta_new = 0
        x_new = 0
        y_new = 0

        #End Week 2 Assignment Code

        return Pose(x_new, y_new, (theta_new + pi) % (2 * pi) - pi)

    def get_ir_distances(self):
        """Converts the IR distance readings into a distance in meters"""

        #Insert Week 2 Assignment Code Here

        ir_distances = [0] * len(
            self.robot.ir_sensors.readings)  #populate this list

        #End Assignment week2

        return ir_distances

    def execute(self, robot_info, dt):
        """Inherit default supervisor procedures and return unicycle model output (x, y, theta)"""
        output = Supervisor.execute(self, robot_info, dt)
        self.tracker.add_point(self.pose_est)
        return self.uni2diff(output)

    def process_state_info(self, state):
        """Update state parameters for the controllers and self"""

        Supervisor.process_state_info(self, state)

        self.parameters.pose = self.pose_est

    def draw(self, renderer):
        """Draw a circular goal, path and ir_sensors"""

        # Draw goal
        renderer.set_pose(
            Pose(self.pose_est.x, self.pose_est.y,
                 pi * self.parameters.goal / 180))
        renderer.set_pen(0)
        #        renderer.draw_line(0.03,0,100,0)
        renderer.draw_arrow(0.05, 0, 0.5, 0, close=True)

        # Draw robot path
        self.tracker.draw(renderer)

        renderer.set_pose(self.pose_est)
        renderer.set_brush(0)
        renderer.draw_ellipse(0, 0, 0.01, 0.01)

        # Draw IR distances
        crosses = array([
            dot(p.get_transformation(), [d, 0, 1]) for d, p in zip(
                self.get_ir_distances(), self.robot.ir_sensors.poses)
        ])

        renderer.set_pen(0)
        for c in crosses:
            x, y, z = c

            renderer.push_state()
            renderer.translate(x, y)
            renderer.rotate(atan2(y, x))

            renderer.draw_line(0.01, 0.01, -0.01, -0.01)
            renderer.draw_line(0.01, -0.01, -0.01, 0.01)

            renderer.pop_state()

    def get_controller_state(self):
        return self.parameters
Пример #2
0
class QBGTGSupervisor(QuickBotSupervisor):
    """QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
    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 set_parameters(self, params):
        """Set parameters for itself and the controllers"""
        QuickBotSupervisor.set_parameters(self, params)
        self.gtg.set_parameters(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

        # Update the trajectory
        self.tracker.add_point(self.pose_est)

    def draw_background(self, renderer):
        """Draw controller info"""
        QuickBotSupervisor.draw_background(self, renderer)

        # Draw robot path
        self.tracker.draw(renderer)

    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 at_goal(self):
        distance_to_goal = sqrt( \
                                (self.pose_est.x - self.parameters.goal.x)**2
                              + (self.pose_est.y - self.parameters.goal.y)**2)

        return distance_to_goal < 0.02

    def ensure_w(self, v_lr):

        v_l, v_r = v_lr

        #Week 3 Assignment Code:

        #End Week 3 Assigment

        return v_l, v_r

    def execute(self, robot_info, dt):
        """Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
        if not self.at_goal():
            output = Supervisor.execute(self, robot_info, dt)
            return self.ensure_w(self.uni2diff(output))
        else:
            return 0, 0
Пример #3
0
class QBGTGSupervisor(QuickBotSupervisor):
    """QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
    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 set_parameters(self,params):
        """Set parameters for itself and the controllers"""
        QuickBotSupervisor.set_parameters(self,params)
        self.gtg.set_parameters(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
        
        # Update the trajectory
        self.tracker.add_point(self.pose_est)

    def draw_background(self, renderer):
        """Draw controller info"""
        QuickBotSupervisor.draw_background(self,renderer)

        # Draw robot path
        self.tracker.draw(renderer)
            
    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 at_goal(self):
        distance_to_goal = sqrt( \
                                (self.pose_est.x - self.parameters.goal.x)**2
                              + (self.pose_est.y - self.parameters.goal.y)**2)
        
        return distance_to_goal < 0.02

            
    def ensure_w(self,v_lr):
      
        v_l, v_r = v_lr
        
        #Week 3 Assignment Code:
                
        #End Week 3 Assigment
        
        return v_l, v_r

    def execute(self, robot_info, dt):
        """Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
        if not self.at_goal():
            output = Supervisor.execute(self, robot_info, dt)
            return self.ensure_w(self.uni2diff(output))
        else:
            return 0,0
Пример #4
0
class QuickBotSupervisor(Supervisor):
    """The QuickBotSupervisor inherits from the superclass 'supervisor.Supervisor' to implement detailed calculations for any inheriting QuickBot supervisor. 

    Most importantly, the QuickBotSupervisor object implements the system functions necessary to operate a QuickBot, namely the uni2diff unicycle to differential motion model conversion, the Jacobian problem, and any other computationally complex interface.

    The UI may use the get_parameters function interface to create docker windows for real-time update of the PID parameters. This is an advanced implementation and is not required for students to properly implement their own supervisors."""

    def __init__(self, robot_pose, robot_info):
        """Initialize internal variables"""
        Supervisor.__init__(self, robot_pose, robot_info)

        # initialize memory registers
        self.prev_left_ticks = robot_info.wheels.left_ticks
        self.prev_right_ticks = robot_info.wheels.right_ticks

        # Create tracker
        self.tracker = Path(robot_pose, 0)

        # Create & set the controller
        self.current = self.create_controller("GoToAngle", self.parameters)

    def init_default_parameters(self):
        """Sets the default PID parameters, goal, and velocity"""
        p = Struct()
        p.goal = 45.0
        p.velocity = 0.2
        p.pgain = 3.0

        self.parameters = p

    def get_ui_description(self, p=None):
        """Returns the UI description for the docker"""
        if p is None:
            p = self.parameters

        return [(("goal", "Target angle"), p.goal), ("velocity", p.velocity), (("pgain", "Proportional gain"), p.pgain)]

    def set_parameters(self, params):
        """Set parameters for itself and the controllers"""
        self.parameters.goal = params.goal
        self.parameters.velocity = params.velocity
        self.parameters.pgain = params.pgain
        self.current.set_parameters(self.parameters)

    def uni2diff(self, uni):
        """Convert from unicycle model to differential model"""
        (v, w) = uni

        # Insert Week 2 Assignment Code Here

        # R = self.robot.wheels.radius
        # L = self.robot.wheels.base_length

        vl = 0
        vr = 0

        # End Week 2 Assignment Code

        return (vl, vr)

    def estimate_pose(self):
        """Update self.pose_est using odometry"""

        # Insert Week 2 Assignment Code Here

        # Get tick updates
        # self.robot.wheels.left_ticks
        # self.robot.wheels.right_ticks

        # Save the wheel encoder ticks for the next estimate

        # Get the present pose estimate
        x, y, theta = self.pose_est

        # Use your math to update these variables...
        theta_new = 0
        x_new = 0
        y_new = 0

        # End Week 2 Assignment Code

        return Pose(x_new, y_new, (theta_new + pi) % (2 * pi) - pi)

    def get_ir_distances(self):
        """Converts the IR distance readings into a distance in meters"""

        # Insert Week 2 Assignment Code Here

        ir_distances = [0] * len(self.robot.ir_sensors.readings)  # populate this list

        # End Assignment week2

        return ir_distances

    def execute(self, robot_info, dt):
        """Inherit default supervisor procedures and return unicycle model output (x, y, theta)"""
        output = Supervisor.execute(self, robot_info, dt)
        self.tracker.add_point(self.pose_est)
        return self.uni2diff(output)

    def process_state_info(self, state):
        """Update state parameters for the controllers and self"""

        Supervisor.process_state_info(self, state)

        self.parameters.pose = self.pose_est

    def draw(self, renderer):
        """Draw a circular goal, path and ir_sensors"""

        # Draw goal
        renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y, pi * self.parameters.goal / 180))
        renderer.set_pen(0)
        #        renderer.draw_line(0.03,0,100,0)
        renderer.draw_arrow(0.05, 0, 0.5, 0, close=True)

        # Draw robot path
        self.tracker.draw(renderer)

        renderer.set_pose(self.pose_est)
        renderer.set_brush(0)
        renderer.draw_ellipse(0, 0, 0.01, 0.01)

        # Draw IR distances
        crosses = array(
            [
                dot(p.get_transformation(), [d, 0, 1])
                for d, p in zip(self.get_ir_distances(), self.robot.ir_sensors.poses)
            ]
        )

        renderer.set_pen(0)
        for c in crosses:
            x, y, z = c

            renderer.push_state()
            renderer.translate(x, y)
            renderer.rotate(atan2(y, x))

            renderer.draw_line(0.01, 0.01, -0.01, -0.01)
            renderer.draw_line(0.01, -0.01, -0.01, 0.01)

            renderer.pop_state()

    def get_controller_state(self):
        return self.parameters
Пример #5
0
class QBGTGSupervisor(QuickBotSupervisor):
    """QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
    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 set_parameters(self,params):
        """Set parameters for itself and the controllers"""
        QuickBotSupervisor.set_parameters(self,params)
        self.gtg.set_parameters(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
        
        # Update the trajectory
        self.tracker.add_point(self.pose_est)

    def draw_background(self, renderer):
        """Draw controller info"""
        QuickBotSupervisor.draw_background(self,renderer)

        # Draw robot path
        self.tracker.draw(renderer)
            
    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 at_goal(self):
        distance_to_goal = sqrt( \
                                (self.pose_est.x - self.parameters.goal.x)**2
                              + (self.pose_est.y - self.parameters.goal.y)**2)
        
        return distance_to_goal < 0.02

            
    def ensure_w(self,v_lr):

        #Week 3 Assignment Code:
                
        #End Week 3 Assigment
        # This code is taken directly from Sim.I.Am week 4
        # I'm sure one can do better.

        v_max = self.robot.wheels.max_velocity
        v_min = self.robot.wheels.min_velocity

        R = self.robot.wheels.radius
        L = self.robot.wheels.base_length

        def diff2uni(vl, vr):
            return (vl + vr) * R / 2, (vr - vl) * R / L

        v, w = diff2uni(*v_lr)

        if v == 0:

            # Robot is stationary, so we can either not rotate, or
            # rotate with some minimum/maximum angular velocity

            w_min = R / L * (2 * v_min);
            w_max = R / L * (2 * v_max);

            if abs(w) > w_min:
                w = copysign(max(min(abs(w), w_max), w_min), w)
            else:
                w = 0

            return self.uni2diff((0, w))

        else:
            # 1. Limit v,w to be possible in the range [vel_min, vel_max]
            # (avoid stalling or exceeding motor limits)
            v_lim = max(min(abs(v), (R / 2) * (2 * v_max)), (R / 2) * (2 * v_min))
            w_lim = max(min(abs(w), (R / L) * (v_max - v_min)), 0)

            # 2. Compute the desired curvature of the robot's motion

            vl, vr = self.uni2diff((v_lim, w_lim))

            # 3. Find the max and min vel_r/vel_l
            v_lr_max = max(vl, vr);
            v_lr_min = min(vl, vr);

            # 4. Shift vr and vl if they exceed max/min vel
            if (v_lr_max > v_max):
                vr -= v_lr_max - v_max
                vl -= v_lr_max - v_max
            elif (v_lr_min < v_min):
                vr += v_min - v_lr_min
                vl += v_min - v_lr_min

            # 5. Fix signs (Always either both positive or negative)
            v_shift, w_shift = diff2uni(vl, vr)

            v = copysign(v_shift, v)
            w = copysign(w_shift, w)

            return self.uni2diff((v, w))
        #return v_l, v_r

    def execute(self, robot_info, dt):
        """Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
        if not self.at_goal():
            output = Supervisor.execute(self, robot_info, dt)
            #print(output)
            return self.ensure_w(self.uni2diff(output))
            #return output
        else:
            return 0,0