示例#1
0
def on_step():
    x, y, theta = io.get_position(cheat=True)
    robot.slime_x.append(x)
    robot.slime_y.append(y)
    checkoff.update(globals())

    # the following lines compute the robot's current position and angle
    current_point = util.Point(x,y).add(robot.initial_location)
    current_angle = theta

    forward_v, rotational_v = drive(robot.path, current_point, current_angle)
    io.set_forward(forward_v)
    io.set_rotational(rotational_v)
示例#2
0
def on_step():
    if not REAL_ROBOT:
        checkoff.update(globals())
    global confident, target_x, target_theta
    sonars = io.get_sonars()

    pose = io.get_position(not REAL_ROBOT)
    (px, py, ptheta) = pose

    if confident:
        if target_theta is None:
            target_theta = util.fix_angle_plus_minus_pi(ptheta+math.pi/2)
        ptheta = util.fix_angle_plus_minus_pi(ptheta)
        if px>target_x+.05 and abs(ptheta)<math.pi/4:
            io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary
        elif px<target_x and abs(ptheta)<math.pi/4:
            io.Action(fvel=0.2,rvel=0).execute()  #drive to desired x location
        elif ptheta<target_theta-.05:
            io.Action(fvel=0,rvel=0.2).execute()  #rotate
        elif sonars[3]>.3:
            io.Action(fvel=0.2,rvel=0).execute()  #drive into spot
        else:
            io.Action(fvel=0,rvel=0).execute()  #congratulate yourself (or call insurance company)
        return

    
    # Quality metric.  Important to do this before we update the belief state, because
    # it is always a prediction
    if not REAL_ROBOT:
        parkingSpaceSize = .75
        robotWidth = 0.3
        margin = (parkingSpaceSize - robotWidth) / 2 
        robot.probMeasures.append(estimate_quality_measure(robot.estimator.belief,
                                                         x_min, x_max, num_states, margin,
                                                         px))
        trueS = discretize(px, (x_max - x_min)/num_states, valueMin = x_min)
        trueS = clip(trueS, 0, num_states-1)
        n = len(robot.probMeasures)
        
    # current discretized sonar reading
    left = discretize(sonars[0], sonarMax/num_observations, num_observations-1)
    if not REAL_ROBOT:
        robot.data.append((trueS, ideal[trueS], left))
    # obsProb
    obsProb = sum([robot.estimator.belief.prob(s) * OBS_MODEL_TO_USE(s).prob(left) \
                   for s in range(num_states)])

    # GRAPHICS
    if robot.g is not None:
        # redraw ideal distances (compensating for tk bug on macs)
        # draw robot's true state
        if not REAL_ROBOT:
            if trueS < num_states:
                robot.g.updateDist()
                robot.g.updateTrueRobot(trueS)
        # update observation model graph
        robot.g.updateObsLabel(left)
        robot.g.updateObsGraph([OBS_MODEL_TO_USE(s).prob(left) \
                                for s in range(num_states)])

    robot.estimator.update(left)
    (location, confident) = confident_location(robot.estimator.belief)

    if confident:
        target_x = (parking_spot-location)*(x_max-x_min)/float(num_states) + px
        print('I am at x =',location,': proceeding to parking space')
        if not REAL_ROBOT:
            checkoff.localized(globals())

    # GRAPHICS
    if robot.g is not None:
        # update world drawing
        # update belief graph
        robot.g.updateBeliefGraph([robot.estimator.belief.prob(s)
                                   for s in range(num_states)])

    # DL6 Angle Controller
    (distanceRight, theta) = sonarDist.get_distance_right_and_angle(sonars)
    if not theta:
       theta = 0
    e = desiredRight-distanceRight
    ROTATIONAL_VELOCITY = Kp*e - Ka*theta
    io.set_forward(FORWARD_VELOCITY)
    io.set_rotational(ROTATIONAL_VELOCITY)
示例#3
0
def on_step():
    if not REAL_ROBOT:
        checkoff.update(globals())
    global confident, target_x, target_theta
    sonars = io.get_sonars()

    pose = io.get_position(not REAL_ROBOT)
    (px, py, ptheta) = pose

    if confident:
        if target_theta is None:
            target_theta = util.fix_angle_plus_minus_pi(ptheta + math.pi / 2)
        ptheta = util.fix_angle_plus_minus_pi(ptheta)
        if px > target_x + .05 and abs(ptheta) < math.pi / 4:
            io.Action(fvel=-0.2,
                      rvel=0).execute()  #drive backwards if necessary
        elif px < target_x and abs(ptheta) < math.pi / 4:
            io.Action(fvel=0.2, rvel=0).execute()  #drive to desired x location
        elif ptheta < target_theta - .05:
            io.Action(fvel=0, rvel=0.2).execute()  #rotate
        elif sonars[3] > .3:
            io.Action(fvel=0.2, rvel=0).execute()  #drive into spot
        else:
            io.Action(fvel=0, rvel=0).execute(
            )  #congratulate yourself (or call insurance company)
        return

    # Quality metric.  Important to do this before we update the belief state, because
    # it is always a prediction
    if not REAL_ROBOT:
        parkingSpaceSize = .75
        robotWidth = 0.3
        margin = (parkingSpaceSize - robotWidth) / 2
        robot.probMeasures.append(
            estimate_quality_measure(robot.estimator.belief, x_min, x_max,
                                     num_states, margin, px))
        trueS = discretize(px, (x_max - x_min) / num_states, valueMin=x_min)
        trueS = clip(trueS, 0, num_states - 1)
        n = len(robot.probMeasures)

    # current discretized sonar reading
    left = discretize(sonars[0], sonarMax / num_observations,
                      num_observations - 1)
    if not REAL_ROBOT:
        robot.data.append((trueS, ideal[trueS], left))
    # obsProb
    obsProb = sum([robot.estimator.belief.prob(s) * OBS_MODEL_TO_USE(s).prob(left) \
                   for s in range(num_states)])

    # GRAPHICS
    if robot.g is not None:
        # redraw ideal distances (compensating for tk bug on macs)
        # draw robot's true state
        if not REAL_ROBOT:
            if trueS < num_states:
                robot.g.updateDist()
                robot.g.updateTrueRobot(trueS)
        # update observation model graph
        robot.g.updateObsLabel(left)
        robot.g.updateObsGraph([OBS_MODEL_TO_USE(s).prob(left) \
                                for s in range(num_states)])

    robot.estimator.update(left)
    (location, confident) = confident_location(robot.estimator.belief)

    if confident:
        target_x = (parking_spot - location) * (x_max -
                                                x_min) / float(num_states) + px
        print('I am at x =', location, ': proceeding to parking space')
        if not REAL_ROBOT:
            checkoff.localized(globals())

    # GRAPHICS
    if robot.g is not None:
        # update world drawing
        # update belief graph
        robot.g.updateBeliefGraph(
            [robot.estimator.belief.prob(s) for s in range(num_states)])

    # DL6 Angle Controller
    (distanceRight, theta) = sonarDist.get_distance_right_and_angle(sonars)
    if not theta:
        theta = 0
    e = desiredRight - distanceRight
    ROTATIONAL_VELOCITY = Kp * e - Ka * theta
    io.set_forward(FORWARD_VELOCITY)
    io.set_rotational(ROTATIONAL_VELOCITY)