The Geometrical Model of the DR

Input data: The DR position a vector 2×1 t:=[tx;ty]; The DR orientation a scalar alfa; Output data: The DR graphical representation

Contents

DR parameters

The points coordinates

p0=[0;0;0;1];
p1=[0.3;0;0;1];
p2=[-0.2;0.2;0;1];
p3=[-0.2;-0.2;0;1];
p4=[0.4;0;0;1];
p5=[0;0.4;0;1];
% Contours and lines
%   Obs:for these simple shape we avoid the incident matrix
L1=[p1,p2,p3,p1];
L2=[p0,p4];
L3=[p0,p5];

Representing the DR in its referential frame

figure
hold
plot(L1(1,:),L1(2,:),'k','LineWidth',2)
plot(L2(1,:),L2(2,:),'r','LineWidth',2)
plot(L3(1,:),L3(2,:),'g','LineWidth',2)
grid
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
axis([-0.5,0.5,-0.5,0.5])

Current plot held

First method: change the pose in the absolute referential frame

Input data:

t=[1;1];
alfa=pi/2;
T0_k=[cos(alfa),-sin(alfa),0,t(1,1);...
      sin(alfa),cos(alfa),0,t(2,1);...
      0,0,1,0;...
      0,0,0,1];
L1k=T0_k*L1;
L2k=T0_k*L2;
L3k=T0_k*L3;
% The new pose
figure
hold
plot(L1k(1,:),L1k(2,:),'k','LineWidth',2)
plot(L2k(1,:),L2k(2,:),'r','LineWidth',2)
plot(L3k(1,:),L3k(2,:),'g','LineWidth',2)
grid
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
axis 'equal'

Current plot held

Second method: change the pose relative to the previous referential frame

Input data:

tk_k1=[0.5;0.5];
alfak_k1=pi/2;
Tk_k1=[cos(alfak_k1),-sin(alfak_k1),0,tk_k1(1,1);...
      sin(alfak_k1),cos(alfak_k1),0,tk_k1(2,1);...
      0,0,1,0;...
      0,0,0,1];
L1k_k1=Tk_k1*L1k;
L2k_k1=Tk_k1*L2k;
L3k_k1=Tk_k1*L3k;
% The new pose
plot(L1k_k1(1,:),L1k_k1(2,:),'k','LineWidth',2)
plot(L2k_k1(1,:),L2k_k1(2,:),'r','LineWidth',2)
plot(L3k_k1(1,:),L3k_k1(2,:),'g','LineWidth',2)
axis 'equal'

Trajectory and Working Volume animation

From the first method we have constructed the function DR_gm(t,alfa) Define a circular trajectory

beta=linspace(0,2*pi,100);
r=2;
t=r*[cos(beta);sin(beta)];
alfa=pi/2+beta;
figure
hold
plot(0,0,'kd','MarkerSize',5)
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
grid
% The DR representation
for i=1:length(beta)
    [L1k,L2k,L3k]=DR_gm( t(:,i),alfa(1,i));
    plot(L1k(1,:),L1k(2,:),'k','LineWidth',2)
    plot(L2k(1,:),L2k(2,:),'r','LineWidth',2)
    plot(L3k(1,:),L3k(2,:),'g','LineWidth',2)
    axis 'equal'
    pause(0.025)
end
traject=r*[cos(0:0.1:2*pi);sin(0:0.1:2*pi)];
comet(traject(1,:),traject(2,:))

Current plot held

Published with MATLAB® R2014a