Exercise 4, Problem 9

To implement the changes, the function was modified as follows:

if krho*rho>0.005
   wR=(1/rR)*(krho*rho-d*(kalph*alph+kbeta*beta));
   wL=(1/rL)*(krho*rho+d*(kalph*alph+kbeta*beta));
   wheelSp=[wR wL];
   crtPose = kinupdate(crtPose,robotpar,ts,wheelSp)';
   i = i+1;
else rho = minDist;
      beta = minAngle;
end;

The results are shown in the figure below:

The red path is the original one, where the robot performs all the movements to reach the requested pose. By adding the condition to stop when the speed is smaller than 0.005, the robot performs less movements and stops before reaching the desired position, as shown by the blue path. The green path is taken by the robot when kβ decreases. The robot still performs a smaller number of movements, as in the previous case, but the final pose is closer to the desired one. However, this comes at the cost of driven distance.