Flatland Challenge Solution

Context and Summary

This is a sample solution for the Flatland challenge. This code is meant to be run with the flatland_no_spawn simulator world (see Meeto Your Neato for instructions on getting the simulator up and running).

Source Code Listing

download source

function position = flatlandSolution()
% to calculate wheel velocities for a given angular speed we need to know
% the wheel base of the robot
wheelBase = 0.235;              % meters
% this is the scaling factor we apply to the gradient when calculating our
% step size
lambda = 0.1;

% setup symbolic expressions for the function and gradient
syms x y;
f = x*y - x^2 - y^2 - 2*x - 2*y + 4;
grad = gradient(f, [x, y]);

% the problem description tells us to the robot starts at position 1, -1
% with a heading aligned to the y-axis
heading = [0; 1];
position = [1; -1];

angularSpeed = 0.1;  % radians / second
linearSpeed = 0.05;  % meters / second

% get setup with a publisher so we can modulate the velocity
pub = rospublisher('/raw_vel');
msg = rosmessage(pub);
% stop the robot's wheels in case they are running from before
msg.Data = [0, 0];
send(pub, msg);
pause(2);

% put the Neato in the starting location
placeNeato(position(1), position(2), heading(1), heading(2));
% wait a little bit for the robot to land after being positioned
pause(2);

% set a flag to control when we are sufficiently close to the maximum of f
shouldStop = false;

while ~shouldStop
    % get the gradient
    gradValue = double(subs(grad, {x, y}, {position(1), position(2)}));
    % calculate the angle to turn to align the robot to the direction of 
    % gradValue. There are lots of ways to do this. One way is to use the
    % fact that the magnitude of the cross product of two vectors is equal
    % to the product of the vectors' magnitudes times the sine of the angle
    % between them. Moreover, the direction of the vector will tell us
    % what axis to turn around to rotate the first vector onto the second.
    % We'll use that approach here, but contact us for more approaches.
    crossProd = cross([heading; 0], [gradValue; 0]);
    
    % if the z-component of the crossProd vector is negative that means we
    % should be turning clockwise and if it is positive we should turn
    % counterclockwise
    turnDirection = sign(crossProd(3));
    
    % as stated above, we can get the turn angle from the relationship
    % between the magnitude of the cross product and the angle between the
    % vectors
    turnAngle = asin(norm(crossProd)/(norm(heading)*norm(gradValue)));
    
    % this is how long in seconds to turn for
    turnTime = double(turnAngle) / angularSpeed;
    % note that we use the turnDirection here to negate the wheel speeds
    % when we should be turning clockwise instead of counterclockwise
    msg.Data = [-turnDirection*angularSpeed*wheelBase/2,
                turnDirection*angularSpeed*wheelBase/2];
    send(pub, msg);
    % record the start time and wait until the desired time has elapsed
    startTurn = rostic;
    while rostoc(startTurn) < turnTime
        pause(0.01);
    end
    heading = gradValue;

    % this is how far we are going to move
    forwardDistance = norm(gradValue*lambda);
    % this is how long to take to move there based on desired linear speed
    forwardTime = forwardDistance / linearSpeed;
    % start the robot moving
    msg.Data = [linearSpeed, linearSpeed];
    send(pub, msg);
    % record the start time and wait until the desired time has elapsed
    startForward = rostic;
    while rostoc(startForward) < forwardTime
        pause(0.01)
    end
    % update the position for the next iteration
    position = position + gradValue*lambda;
    % if our step is too short, flag it so we break out of our loop
    shouldStop = forwardDistance < 0.01;
end

% stop the robot before exiting
msg.Data = [0, 0];
send(pub, msg);

end