Write a Matlab code to draw the robot given its current position and orientation

Question

Write a Matlab code to draw the robot given its current position and orientation on the global X,Y plane. Assume X is positive to the right, Y is positive up, and the orientation of the robot is measured with respect to the X axis. Make sure your orientation is always expressed in the range [-Pi, PI].

Use rectangles for the robot body (filled with red color) and for the wheels (filled with black color).

Make sure you use a function that you can call from your main file. This function should update the drawing of the robot given its global position X,Y, and its orientation Phi.

Suggestions:

• Add the following extra fields to the robot struct: robot.X, robot.Y, robot.Phi

• To draw the robot, learn how to use Matlab’s hgtransform.

Explanation / Answer

ANSWER:

The matlab code consists of three parts - 'main.m', 'drawrobot.m' and 'fillRectangle.m'

Matlab code:

main.m:

%Start of code
close all
clear all
robot.rl = 0.2; %Length of robot
robot.rb = 0.1; %Breadth of robot
robot.wl = 0.04; %Length of wheel
robot.wb = 0.02; %Breadth of wheel

omega = pi/5; %Angular velocity of robot along the circular path
dt = 0.01; %Time step
T = 2*pi/omega; %End time
t = 0:dt:T; %Time vector
theta = omega*t-pi/2; %Angular position of robot
x = cos(theta); %X-position of robot
y = 1+sin(theta); %Y-position of robot
for i = 1:length(t)
robot.X = x(i); %X-position of robot at given time instance
robot.Y = y(i); %Y-position of robot at given time instance
robot.phi = theta(i)+pi/2; %Angular-position of robot at given time instance
hold off
drawrobot(robot) %Draw robot
s = strcat('t = ',num2str(t(i)),'secs'); %Show time
text(0,2.25,s)
xlim([-1.5 1.5])
ylim([-0.5 2.5])
pause(0.001)
end
%End of code

drawrobot.m:

%Draw robot at given position
function drawrobot(robot)

%%
%Start of code
%Calculate the positions of center points of all four wheels
phiw = atan((robot.rb+robot.wb)/(robot.rl-robot.wl));
lw = ((robot.rl-robot.wl)^2+(robot.rb+robot.wb)^2)^0.5/2;
phiw1 = robot.phi+phiw;
phiw2 = robot.phi-phiw;
phiw3 = robot.phi+pi+phiw;
phiw4 = robot.phi+pi-phiw;

%Poisitons of center points of all four wheels
xw1 = robot.X+lw*cos(phiw1);
xw2 = robot.X+lw*cos(phiw2);
xw3 = robot.X+lw*cos(phiw3);
xw4 = robot.X+lw*cos(phiw4);
yw1 = robot.Y+lw*sin(phiw1);
yw2 = robot.Y+lw*sin(phiw2);
yw3 = robot.Y+lw*sin(phiw3);
yw4 = robot.Y+lw*sin(phiw4);

%Draw robot body
fillRectangle(robot.X,robot.Y,robot.phi,robot.rl,robot.rb,'r')
hold on
%Draw robot wheels
fillRectangle(xw1,yw1,robot.phi,robot.wl,robot.wb,'k')
fillRectangle(xw2,yw2,robot.phi,robot.wl,robot.wb,'k')
fillRectangle(xw3,yw3,robot.phi,robot.wl,robot.wb,'k')
fillRectangle(xw4,yw4,robot.phi,robot.wl,robot.wb,'k')
%End of code

fillRectangle.m:

%Start of code
%Draws rectangle at given positions with given angle and dimensions and
%fills it with specified color
%X = X-position of center point of rectangle
%Y =Y-position of center point of rectangle
%phi = Angle of orientation of rectangle
%l = length of rectangle
%b = breadth of rectangle

function fillRectangle(X,Y,phi,l,b,color)

phir = atan(b/l);
lr = (l^2+b^2)^0.5/2;
phi1 = phi+phir;
phi2 = phi-phir;
phi3 = phi+pi+phir;
phi4 = phi+pi-phir;

xr = [X+lr*cos(phi1) X+lr*cos(phi2) X+lr*cos(phi3) X+lr*cos(phi4)];
yr = [Y+lr*sin(phi1) Y+lr*sin(phi2) Y+lr*sin(phi3) Y+lr*sin(phi4)];

fill(xr,yr,color)
%End of code

Related Questions

Navigate

• Browse (All)
• Browse W
• Subjects

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.