Robotics Lab

Lecture #4

Purpose of This Lecture:

- Learn how to draw axis of a frame or multiple frames in the same figure and draw shapes
referred to them.
- Learn how to solve problems such as : representing a shape on another frame.

Programs Practice:

Part 1: Programs
The following functions are used to draw the frame axis appearance, its orientation and
position.

function vectarrow(p0,p1,x)
x0 = p0(1); y0 = p0(2); z0 = p0(3);
x1 = p1(1); y1 = p1(2); z1 = p1(3);
hold on;
if x==1
plot3([x0;x1],[y0;y1],[z0;z1],'color','red','LineWidth', 4);
% Draw a line between p0 and p1
elseif x==2
plot3([x0;x1],[y0;y1],[z0;z1],'color','black','LineWidth', 4);
% Draw a line between p0 and p1
else
plot3([x0;x1],[y0;y1],[z0;z1],'color','blue','LineWidth', 4);
% Draw a line between p0 and p1
end

end
………………………………………………

function draw_axis(H10)
p0 =[H10(1,4) H10(2,4) H10(3,4)];
H10=H10*0.5;
for i=1:3
p1 = [p0(1)+H10(1,i) p0(2)+H10(2,i) p0(3)+H10(3,i)];
vectarrow(p0,p1,i);
end

end

………………………………………………

1
Notes :
1- draw_axis function manipulates the homogenous matrix to get the frame axis.
2- Vectorarrow function uses the data from draw_axis to draw the frame axis.
3- The object is to draw all the frames in the following figure:-

Where d0=0.8 , d1=2.2 , h=2 , a=0.6 , Ɵ=48.19, ᵩ= 63.43 .

=( )

2
 =( )

=( )

Program number 1:
function exp0()

H00=eye(4);

H10=[cosd(-90) 0 sind(-90) 3;

0 1 0 0;

-sind(-90) 0 cosd(-90) 0;

0 0 0 1];

p1=[0;2;1.2;1];

p0=H10*p1;

hold on;

draw_axis(H00);

draw_axis(H10);

plot3(p0(1),p0(2),p0(3),'o', 'MarkerEdgeColor', 'black', 'MarkerFaceColor', 'black');

axis([-0.5 4.5 -0.7 2.5 -0.5 2.5]),grid;

xlabel('X');

ylabel('Y');

zlabel('Z');

end

3
Testing program 1:
clear; clc;
exp0(); % Or exp2(); when testing exp4()…

Program number 2:
function ex1()
H00=eye(4);
H10=[cosd(-90) 0 sind(-90) 3;
0 1 0 0;
-sind(-90) 0 cosd(-90) 0;
0 0 0 1];
H20=[cosd(153.43) -sind(153.43) 0 0.8;
sind(153.43) cosd(153.43) 0 0;
0 0 1 0; 0 0 0 1];
Hw2=[1 0 0 0;
0 cosd(131.8100) -sind(131.8100) -0.4472;
0 sind(131.8100) cosd(131.8100) 1.6;
0 0 0 1];
p0=[1.8;2.0;0;1];
hold on;
draw_axis(H00); draw_axis(H10);
draw_axis(H20); draw_axis(H20*Hw2);
plot3(p0(1),p0(2),p0(3),'o', 'MarkerEdgeColor', 'black', 'MarkerFaceColor', 'black');
axis([-0.5 4.5 -0.7 2.5 -0.5 2.5]),grid;
xlabel('X'); ylabel('Y'); zlabel('Z');

end

……………………
Notes :
Program number 1 will project frame #0 and #1 with #0 as the base frame.
program number 2 will create all of the frames of the figure with frame #0 as the base frame.
program number 3 will create all the frames of the figure with frame #1 as the base frame .

4
Program number 3:
function ex2()

H11=eye(4);

H10=[cosd(-90) 0 sind(-90) 3;

0 1 0 0;

-sind(-90) 0 cosd(-90) 0;

0 0 0 1];

H20=[cosd(153.43) -sind(153.43) 0 0.8;

sind(153.43) cosd(153.43) 0 0;

0 0 1 0; 0 0 0 1];

Hw2=[1 0 0 0;

0 cosd(131.8100) -sind(131.8100) -0.4472;

0 sind(131.8100) cosd(131.8100) 1.6;

0 0 0 1];

H01=H10^-1;

H21=H01*H20;

Hw1=H21*Hw2;

p0=[1.8;2.0;0;1];

p1=H01*p0

hold on;

draw_axis(H11); draw_axis(H01);

draw_axis(H21); draw_axis(Hw1);

plot3(p1(1),p1(2),p1(3),'o', 'MarkerEdgeColor', 'black', 'MarkerFaceColor', 'black');

axis([-0.5 4.5 -0.7 2.5 -0.5 2.5]),grid;

xlabel('X'); ylabel('Y'); zlabel('Z');

end