Exercise 4, Problem 1

The following function moves the robot from the initial position (0, 0, 0) with the values given as an input:

function [a b c] = moveWith(relativePose)

global pose initpose ncolor;
pose = initpose;
x = relativePose(1);
y = relativePose(2);
th = relativePose(3);
a = atan2(y, x);
b = sqrt(x^2+y^2);
c = th-a;
array = turn(a,1);
for i=1:size(array)
    a2=[array(i,1) array(i,1)+0.1*cos(array(i,3))];
    b2=[array(i,2) array(i,2)+0.1*sin(array(i,3))];     line(a2,b2,'Color',ncolor);
end
array = forward(b,1);
plot(array(:,1),array(:,2),'Color',ncolor,'LineWidth',2);
array = turn(c,1);
for i=1:size(array)
    a2=[array(i,1) array(i,1)+0.1*cos(array(i,3))];
    b2=[array(i,2) array(i,2)+0.1*sin(array(i,3))];
    line(a2,b2,'Color',ncolor);
end
end

The outputs a, b and c are the movements (rotation and translation) needed to be performed in order to reach the desired position.