Exercise 10, Problem 2

Problem 2 Generation of target poses for driving in a box.

The box has 5 points: the 4 corners and the starting point (pose) in mainloop.m file:

places = [  [0; 0; 0] [0.5; 0.5; pi] [-0.5; 0.5; -pi/2] [-0.5; -0.5; 0] [0.5; -0.5; pi/2] [0.5; 0.5; pi] ];

The sequence of the points with exception handling in mainloop.m file:

if exist('nextPose','var') == 0 %
  nextPose = 1;
elseif nextPose >= length(places(1,:))
  nextPose = 1;
else
  nextPose = nextPose + 1;
end

Constants setup in file constants.m:

simulation = true;
noOfIter = 16; %The number of simulation iterations

Exercise 10, Problem 2.1

Simulate Boxdriving. Run the framework and see that the boxdrive works in simulation.
The result of the simulation reveals the errors in odometry:

Exercise 10, Problem 1.1

Calculation of targetpose in odometry coordinates.

function out = trans(transform,targetPose)
% out <-> odoTargetPose (notation)
% odoTargetPose = TRANS(transform,targetPose)
% Transform a given point in world coordinates (targetPose) to odometry
% coordinates, using the origo of the odometry coordinates in world
% coordinates (transform).
%calculation of targetpose in ordinary coordinates
    t=transform; %for shorter name
    tMatrix=[cos(t(3)) -sin(t(3))  0; sin(t(3))  cos(t(3))  0; 0 0 1];
    temp = tMatrix*targetPose ;
    out = temp + t;
    out(3) = normalizeAngle(out(3));
end

Exercise 10, Problem 1

Calculation of transformation from world coordinates to odometry coordinates.

function transform = findTransform(odoPose, pose)
% transform = FINDTRANSFORM(odoPose,pose)
% Find the transformation from the world coordinates to the odometry
% coordinates given a pose in the odometry coordinates (odoPose) and the
% same point in the world coordinates (pose). The output (transform) is
% simply the origo of the odometry coordinates in the world coordinates
  theta =  normalizeAngle(odoPose(3)-pose(3));
  tMatrix = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
  transform = -tMatrix*pose + odoPose;
  transform(3) = normalizeAngle(transform(3));
end

Where we have used the angle normalization:

function outAngle = normalizeAngle(inAngle)
    inAngle  = inAngle + 2*pi;
    outAngle = mod(inAngle,2*pi);
    outAngle = outAngle -2*pi;
end