Exercise 4, Problem 7

The function below implements the algorithm:

function retur = fbMotion(pose1,pose2)

global robotpar ts;
minDist = 0.01;
minAngle = 0.02;
krho = 0.3;
kalph = 0.8;
kbeta = -0.15 ;
d = robotpar(1);
rR = robotpar(2);
rL = robotpar(3);
maxIter = 10000;
crtPose = pose1;
rho = 10;
beta = 10;
i=1;
while ((( rho > minDist)) || (abs(beta) > minAngle) ) && ( i < maxIter)
deltaX = pose2(1)-crtPose(1);
deltaY = pose2(2)-crtPose(2);
theta  = crtPose(3);
rho = sqrt(deltaX^2 + deltaY^2);
alph = -theta + atan2(deltaY,deltaX);
if alph > pi
   alph = alph - 2*pi;
else
   if alph < -pi
      alph = alph + 2*pi;
   end  
end
beta = -theta - alph + pose2(3);
if beta > pi
   beta = beta - 2*pi;
else
   if beta < -pi
      beta = beta + 2*pi;
   end  
end
crt(i,:)=crtPose;
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;
end
retur = crt;
end