Exercise 4, Problem 11

Given that the original position of the robot is always set to be (0,0,0) , we took the final position (-0.5, -0.5, 0), in order to compare with the previously simulated case. The result was more than satisfactory.

Exercise 4, Problem 10

We transposed the program to smr-cl:
minDist=0.01
minAngle=0.02

krho=0.3
kalph=0.8
kbeta=-0.35

d=0.26
rR=0.035
rL=0.035
maxIter=10000

pose2x=-0.5
pose2y=-0.5
pose2th=0
i=1

log "$odox" "$odoy" "$odoth"

label "update"
deltaX=pose2x-$odox
deltaY=pose2y-$odoy
theta=$odoth
rho=sqrt(deltaX*deltaX + deltaY*deltaY)
alph=normalizeanglerad(-theta + atan2(deltaY,deltaX))
beta=normalizeanglerad(-theta - alph + pose2th)

if((krho*rho) < 0.005) "finish"

vr=krho*rho-d*(kalph*alph+kbeta*beta)
vl=krho*rho+d*(kalph*alph+kbeta*beta)
motorcmds vr vl
i=i+1

if(((rho>minDist)|(abs(beta)>minAngle))&(i<maxIter)) "update"

label "finish"
stop

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.

Exercise 4, Problem 8

The movements of the robot from all the poses given are shown in the figure below:

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

Exercise 4, Problem 4

The script presented in the previous problem has been tested on the robot and the position and angle values recorded by the robot were stored in a file. We have implemented the following program in MATLAB:

[x,y,th] = textread('log.txt','%f %f %f');
hold on;
i=1;
while i <= length(x)
    a=[x(i) x(i)+0.5*cos(th(i))];
    b=[y(i) y(i)+0.5*sin(th(i))];
    line(a,b)
    i=i+1;
end  
plot(x,y,'Color','red','LineWidth',3);

The program takes the values stored in the file and plots the trajectory (the red line) and the corresponding orientation of the robot at each point (the blue lines).

Exercise 4, Problem 3

The implemented algorithm is the following:

log "$odox" "$odoy" "$odoth"
x=2
y=-0.5
a=atan2(y,x)
b=sqrt(x*x+y*y)
th=3.141
c=th-a
turn a "rad"
fwd b
turn c "rad"
stop

Exercise 4, Problem 2

The visualisation of the movements for the given cases is presented in the following figure:

The implementation was realised as in previous problems. The orientation of the robot at the starting position can not be seen in all the cases due to overlapping of the graphs.

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.