Exercise 3, Problem 9

The function has the following form:


function move = forward(distance,speed)
   global ts pose;
   size = round(abs(distance)/(speed*ts));   %size of the array
   array = zeros(size,3);                              %initializing the array
   th = pose(3);

   Rinv = [cos(th)  -sin(th)  0;
               sin(th)    cos(th)  0;
                   0           0       1];
    
   M = [(distance/abs(distance))*speed;
                                0;
                                0];
    
   for i=1:size
       array(i,:) = pose' + Rinv*M*ts;
       pose = array(i,:);
   end

   move = array;
end

Both positive and negative values can be given for the distance and are correctly interpreted. The sign is taken into consideration when building the M array.