How can I make the trajectory of robot with distance and angle values.

Hi,

I am confusing like how to make the trajectory of my robot when I have just the distance values and angles values. It means that robot start from let suppose position zero and it travel 10 inches after that it turn 25 degree and then move 4 inches and then turn 25 degree. I have this data and I am struggling in plotting the trajectory. If any one help me in this regards is very kind of him.

Thank you

[darova]

Can you show something more?

[ABdanyia]

Hi,

Thank you for reply, So this is my distance values variable "path".

path=[0 25 45 34 12 22 13 17 48 99];

I roughly made the sketch of how it looks like

[ABdanyia]

As darova helped mw with this code, so the thing i want to solved but i am unable to, the problem is the red mark which

I did is not right, the previous turn is fine because of 26 degree turn, but the other turn which i marked should not be there rather the line should move straight after 26 degree turn Can any one help me with it.

clc,clear
path=[800,50,30];
turn=[1,-1,1].*(1:length(path))
%q=(1:length(path));
angle = 26*turn;
x2 = path.*cosd(angle);
y2 = path.*sind(angle);
x = cumsum(x2);
y = cumsum(y2);
t = cumsum(path);
t1 = linspace(0,t(end),100);
x1 = interp1(t,x,t1);
y1 = interp1(t,y,t1);
plot(x,y,'or')
axis([min(x1) max(x1) min(y1) max(y1)])
hold on
for i = 1:length(x1)-1
    plot(x1(i:i+1),y1(i:i+1),'.-b')
    pause(0.1)
end
hold off

Answer 1

[darova]

I maded simple example for you

L1 = 5;
L2 = 3;
L3 = 4;
t1 = linspace(30,120,20)';
t2 = linspace(10,90,20)';
t3 = linspace(60,0,20)';
x1 = L1*cosd(t1);
y1 = L1*sind(t1);
x2 = x1 + L2*cosd(t2);
y2 = y1 + L2*sind(t2);
x3 = x2 + L3*cosd(t3);
y3 = y2 + L3*sind(t3);

for i = 1:length(t1)
    plot([0 x1(i) x2(i) x3(i)],[0 y1(i) y2(i) y3(i)],'.-b')
    axis([-1 1 -1 1]*10)
    pause(0.1)
end

[ABdanyia]

why this code gives error in my matlab 2015a.

Error using are (line 21)
Nonsquare A matrix

[darova]

Please show me this matrix

[ABdanyia]

Here is this are function.

function X = are(A,B,C)
%ARE  Algebraic Riccati Equation solution.
%
%    X = ARE(A, B, C) returns the stablizing solution (if it
%    exists) to the continuous-time Riccati equation:
%
%           A'*X + X*A - X*B*X + C = 0
%
%    assuming B is symmetric and nonnegative definite and C is
%    symmetric.
%
%    See also  RIC, CARE, DARE.

%       M. Wette, ECE Dept., Univ. of California 5-11-87
%       Revised 6-16-87 MW
%               3-16-88 MW
%   Copyright 1986-2002 The MathWorks, Inc. 

%  -- check for correct input problem --
[nr,nc] = size(A); n = nr;
if (nr ~= nc), error('Nonsquare A matrix'); end;
[nr,nc] = size(B);
if (nr~=n | nc~=n), error('Incorrectly dimensioned B matrix'); end;
[nr,nc] = size(C);
if (nr~=n | nc~=n), error('Incorrectly dimensioned C matrix'); end;

% Following is much faster than before
[q,t] = schur([A -B; -C -A']);
[q,t] = rsf2csf(q,t);

tol = 10.0*eps*max(abs(diag(t)));   % ad hoc tolerance
ns = 0;
%
%  Prepare an array called index to send message to ordering routine 
%  giving location of eigenvalues with respect to the imaginary axis.
%  -1  denotes open left-half-plane
%   1  denotes open right-half-plane
%   0  denotes within tol of imaginary axis
%
index = [];  
for i = 1:2*n,
    if (real(t(i,i)) < -tol),
        index = [ index -1 ];
    ns = ns + 1;
    elseif (real(t(i,i)) > tol),
    index = [ index 1 ];
    else,
    index = [ index 0 ];
    end;
end;
if (ns ~= n), error('No solution: (A,B) may be uncontrollable or no solution exists'); end;
[q,t] = schord(q,t,index);
X = real(q(n+1:n+n,1:n)/q(1:n,1:n));

[darova]

What is matrix X?

[ABdanyia]

Well I dont know about this are function. Now instead of are , it work when putting sind function.

[ABdanyia]

HI,

I try something with that. I am thinking to make the triangle with the distance and angle and find the points with that, so in this way plotting the hypotenuse, I find the triangular sides and then connect these points. Thats what I am trying to do in this code but could not able to plot successfully. :-(

path=[0 25 45 34 12 22 13 17 48 99];
angle=25;
x=zeros(1,length(path));
y=zeros(1,length(path));
for i=2:length(path)
x(i)=(path(i)*sind(angle))+path(i-1);
y(i)= path(i);
end

figure
for i=1:length(k);
   
   plot([x(i),y(i)],'-o')
   pause(1)
   hold on
   drawnow
end

[darova]

Can you explain what do these lines mean?

path=[0 25 45 34 12 22 13 17 48 99];
angle=25;

[ABdanyia]

This path is the distances, So let me explain this , path array contains the number of distance robot move before rotate with an angle of 25. Angle 25 is fixed. SO the robot start from initial point having distance zero at first, the robot move 25 mili meter distance after that robot rotate at an angle of 25, then the sensor started again for distance then robot move 45 mili meter of distance and then rotate 25 degree. So path is just a sample array this array of path may contain upto 100 values. My robot is an obstacle avoidence robot and it contain distance sensor it gives value untill the object come or the robot rotate at an angle of 25.

[darova]

Ok. I understand

clc,clear

path=[0 25 45 34 12 22 13 17 48 99];

angle = 25*(1:length(path));
x = path.*cosd(angle);
y = path.*sind(angle);
x = cumsum(x);
y = cumsum(y);
t = cumsum(path);

t1 = linspace(0,t(end),100);
x1 = interp1(t,x,t1);
y1 = interp1(t,y,t1);

plot(x,y,'or')
axis([min(x1) max(x1) min(y1) max(y1)])
hold on
for i = 1:length(x1)-1
    plot(x1(i:i+1),y1(i:i+1),'.-b')
    pause(0.1)
end
hold off

[ABdanyia]

Thank you so much it work. Thank you for your time.

[ABdanyia]

Hi,

Sorry to disturb you again, Now I am runing this code according to my scenerio, now i have a problem is that sometimes is gave 25 degree but sometimes not also i have to move robot in both direction means that for left the angle is 25 degree and for right it should be -25 degree, so now i am facing this problem, i am also done by multiplying some values with negative but it does not show right graph. i f anyone help me in this regard, i will be grateful.

Thank you

[darova]

Add this line

angle = 25*(1:length(path));
angle([1 3 5]) = -25;           % indices of array you want to be negative (-25)

[ABdanyia]

Hi,

Thank you for your reply, i done the changes and run it, the problem is now the black highlighted part not

giving the 25 degree.

clc,clear
path=[80,50,30,35];
angle = 25*(1:length(path));
angle([1 3]) = -25;  
%q=(1:length(path));
%angle = 26*turn;
x2 = path.*cosd(angle);
y2 = path.*sind(angle);
x = cumsum(x2);
y = cumsum(y2);
t = cumsum(path);
t1 = linspace(0,t(end),100);
x1 = interp1(t,x,t1);
y1 = interp1(t,y,t1);
plot(x,y,'or')
axis([min(x1) max(x1) min(y1) max(y1)])
%axis([-100 1000 -100 1000])
hold on
for i = 1:length(x1)-1
    plot(x1(i:i+1),y1(i:i+1),'.-b')
    pause(0.1)
end
hold off

[darova]

Ok. I understood. I'll try to crrect it tomorrow

[darova]

Please see my new improved script (attached)

[ABdanyia]

Yeah it work, Thank you so much for your time really appriciated. God bless you