HW2

Part1

Report:

My initial thoughts about the assignment, were that since all we are supposed to do is to calculate the linear and angular velocity of the differential driver, we could simply calculate those numbers, and use the simulator function from the previous homework to move and plot the robot, and check with we reach the goal after every step. So I rewrote and appropriated the diffDrive function from homework 5 to look like this:

function [ x, y, theta ] = diffDrive(x0, y0, theta0, v, omega, t, delta)

%diffDrive simulates the movement of a differencial driving robot

x = zeros([1, t]);

y = zeros([1, t]);

theta = zeros([1, t]);

x(1) = x0;

y(1) = y0;

theta(1) = theta0 + omega * delta;

for i = 2 : t

x(i) = x(i-1) + (v * cos(theta(i-1)) * delta);

y(i) = y(i-1) + (v * sin(theta(i-1)) * delta);

theta(i) = theta(i-1) + omega*delta ;

end

plot(x , y);

end

This however, presented a number of challenges. Most importantly, I had to plot every move, on top of the graph from the last move. But I researched and learned how to do that in matlab, and so did all of the functions, by calling the diffDrive. But the other problem I learned later on, would be that I could only plot in the (x,y) plane with this function, and not be able to plot either x(t), y(t) or theta(t). And so, I re-did the first and the third functions, holding off on the second one, since we need to see the second one in (x,y) plane anyways, in order for us to verify the success of the move in merging with the line. Bellow, you will find all the current versions of the functions, and a few plots showing their success:

1) goToPoint:

This function simply calibrates the velocities of the diffdriver to make him reach the goal.

function [currentx,currenty,theta] = goToPoint(x0, xg)

%Moving a differential drive robot from point x0 to the goal point xg

t = 100; %max time

x = zeros([1, t]);

y = zeros([1, t]);

theta = zeros([1, t]);

current = x0;

Kv = 0.2; %<-----sample number

Kh = 0.9; %<-----sample number

error = 0.01;

x(1) = x0(1);

y(1) = x0(2);

theta(1) = x0(3);

for i= 2 :t

%computing part

thetaStar = atand((xg(2)-y(i-1))/(xg(1)-x(i-1)));

omega = Kh*(thetaStar - theta(i-1));

v =Kv * sqrt(((xg(1)-x(i-1))^2)+((xg(2)-y(i-1))^2));

%moving part

theta(i)= theta(i-1)+omega;

x(i)= x(i-1)+(v * cosd(theta(i-1)));

y(i)= y(i-1)+(v * sind(theta(i-1)));

%checking part

if(abs(xg(1)-x(i))<=error)&&((abs(xg(2)-y(i)))<=error)

x = x(1:i);

y = y(1:i);

theta = theta(1:i);

figure

plot(x,y)

break;

end

__________________________________________________________________________

2)followLine:

This function calls on 2 other functions, diffDriver that I discussed already, and one more function called drawLine that I designed that draws a single line given [a,b,c], so we can see on the same plot if our robot is following the line or not.

Sample output:

goToPoint([0,0,0],[10,10]) with Kv = 0.2, Kh=0.9

reducing Kh would increase the smoothness of the graph, as long as Kh was still bigger than Kv, as can be seen below:

function [x,y,theta] = followLine( x0, l )

%Robot goes from x0 to line l=[a,b,c] (describing ax+by+c=0) and aligns

%with it.

kd = 0.5;

kh = 0.5;

aligned = false;

figure

hold on

thetaStar = atan((-l(1))/l(2));

drawLine(l(1),l(2),l(3));

currentPosition = x0;

while ~aligned

d = dot(l, [currentPosition(1),currentPosition(2),1])/sqrt((l(1)^2)+(l(2)^2));

alphaD = -kd * d;

alphaH = kh*(thetaStar-currentPosition(3));

omega = alphaD + alphaH;

[x,y,theta] = diffDrive(currentPosition(1),currentPosition(2),currentPosition(3),0.5,omega,2,1);

currentPosition = [x(end),y(end),theta(end)];

aligned = (abs(dot(l,[currentPosition(1),currentPosition(2),1])/sqrt((l(1)^2)+(l(2)^2)))< 0.01 && (abs(thetaStar-currentPosition(3))< 0.01));

end

hold off

end

goToPoint([0,0,0],[10,10]) with Kv = 0.1, Kh=0.1

__________________________________________________________________

The helper function:

function [ d ] = drawLine( a,b,c )

%Draws a line from the given format of zx+by+c=0

x = linspace(0,5);

y = ((-a*x)/b) - (c/b);

plot(x,y);

end

Sample:

followLine([0,0,0],[-2,1,5])

And from a different origin, with much lower PID values:

__________________________________________________________________________

3)goToPose

With this function I switched to using degrees instead of radians, since it would make it easier for me to

function [ x,y,theta ] = goToPose( x0,xg )

t = 100; %max time

time = zeros([1,t]); %max time

x = zeros([1, t]);

y = zeros([1, t]);

theta = zeros([1, t]);

kro = 0.4;

kalpha = 0.9;

kbeta =0.2;

error = 0.01;

x(1) = x0(1);

y(1) = x0(2);

theta(1) = x0(3);

time(1)=1;

for i= 2 :t

time(i)=i;

%Computing part

ro = sqrt(((xg(1)-x(i-1))^2)+((xg(2)-y(i-1))^2));

alpha = -theta(i-1)+ atan2d(xg(2)-y(i-1),xg(1)-x(i-1));

beta = -theta(i-1)-alpha;

v = kro*ro;

omega = kalpha*alpha + kbeta*beta;

%Moving part

theta(i)= theta(i-1)+omega;

x(i)= x(i-1)+(v * cosd(theta(i-1)));

y(i)= y(i-1)+(v * sind(theta(i-1)));

%Checking part

if(abs(xg(1)-x(i))<=error)&&((abs(xg(2)-y(i)))<=error)

x = x(1:i);

y = y(1:i);

theta = theta(1:i);

time=time(1:i);

figure

plot(x,y);

figure

plot(time,x);

break;

end

followLine([5,5,0],[1,2,3])

___________________________________________________________________________

goToPose([0,0,0],[10,10,0]) results in the following graphs:

XY:

X(t):

Y(t):

With this function, playing with the PIDs means the world. Putting the wrong values may result in... well see for yourself:

And Theta(t):

That is for the same call to the function as the right side, but with bad, and inappropriate values for the PIDs. Have to admit, it is somewhat aesthetically pleasing.

__________________________________________________________________________

Part 2:

I do hope you forgive me, but I had to modify "navigationField script.m " to make it work with my code. I can only assume there have been software changes to matlab in recent years and that is the reason for inconsistencies. So this is the modified version of the navigationField script:

close all; clear;

x0 = [0; 0]; % initial pose is set

x(:,1) = x0;

% obstacles are set

xo(:,1) = [40; 30];

xo(:,2) = [70; 20];

d0 = 10;

xg = [100; 60];

figure; hold on;

axis([0 100 0 100]);

plot(xg(1), xg(2), 'r*'); % goal position

plot(x(1,1),x(2,1),'r.') % start position

%Those circle funcitons didn't work, re-placing with code I found online:

circle(xo(1,1),xo(2,1),d0);

circle(xo(1,2),xo(2,2),d0);

x(:,1) = x0; % starting position

%xg = [100; 20];

x = goToAvoid(x0,xg,xo);

%figure;

hold on;

axis([0 100 0 100]);

plot(x(1,:),x(2,:))%,'r.')

And now for the main event, here is the goToAvoid function:

function [ x ] = goToAvoid( x0,xg,xo )

%navigates a point robot from x0, to xg, avoiding all obsticals on list of

%vectors xo.

t = 100; %max time

x = zeros(2,10);

xi = 0.05;

ro0 = 22;

k=800 ;

goalDistance = sqrt(sum((xg-x0).^2));

x(:,1) = x0;

%Ua = xi*(xg-pose);

obsticleDistance = zeros([1:length(xo)]); %initializing

%looping for ever step.

for i= 2:t

Ua = xi*(xg-x(:,i-1)); %attractive force

Ur = 0; %repulsive force

for j = 1:length(xo) %for ever obsticle

obsticleDistance(j)= distance(x(:,i-1),xo(:,j));

if(obsticleDistance(j) <= ro0) %if we are close enough

Ur = Ur+ k*((1/ro0)-(1/obsticleDistance(j)))*(1/obsticleDistance(j)^2)*(x(:,i-1)-xo(:,j))

end

if(Ur ~=0) %if we are not far enough, add the Ur

Ua = Ua - Ur;

end

v = Ua;

x(:,i) = x(:,i-1)+v;

goalDistance = sqrt(sum((xg-transpose(x(:,i))).^2));

if (goalDistance <= 0.01)

x = x(1:i);

break;

end

%plot(x(1,:),x(2,:))

end

A very big example:

xo(:,1) = [45; 30];

xo(:,2) = [10; 20];

xo(:,3) = [50;50];

xo(:,4) = [50;80];

___________________________________________________________________________

The most important modification to the code was to draw the circular obstacles. Matlab does not support "plot_circle" functions that were used in the scrip, so I had to replace that function with this function I found on one of matlab's community forums:

function circle(x,y,r)

%x and y are the coordinates of the center of the circle

%r is the radius of the circle

%0.01 is the angle step, bigger values will draw the circle faster but

%you might notice imperfections (not very smooth)

ang=0:0.01:2*pi;

xp=r*cos(ang);

yp=r*sin(ang);

plot(x+xp,y+yp);

end

You can find the source of this function here:

https://www.mathworks.com/matlabcentral/answers/3058-plotting-circles

in addition, I changed the final lines, since the way they were, it would design the movements of the robot as dots, and on 2 different figures.

I also introduced a very short helper function, that would allow me to find the distance between any 2 given points. :

function [ c ] = distance( a,b )

c = sqrt(sum((a-b).^2));

end

Thankfully, the function works!

I was surprised at needing to increase the PID of repulsion to such a high level (k=800), but I guess if it works, it works. Here are 3 different situations:

Example 1:

% obstacles are at

xo(:,1) = [40; 30];

xo(:,2) = [70; 40];

Example 2:

% obstacles are set

xo(:,1) = [25; 10];

xo(:,2) = [70; 40];

Example 3:

% obstacles are set

xo(:,1) = [40; 30];

xo(:,2) = [70; 20];

As you can see, it comes close, but still no collisions.