function [Tip] = Rhino(Q, TIP)

% Q = [q1; q2, q3; q4, q5]

N = 5;

alpha = [0, -pi/2, 0, 0, -pi/2];
a = [0, 0, 50, 50, 0];
d = [50, 0, 0, 0, 5];

T01a = Transform(alpha(1), a(1), 0,    Q(1));
T01  = Transform(alpha(1), a(1), d(1), Q(1));
T12a = Transform(alpha(2), a(2), 0,    Q(2));
T12  = Transform(alpha(2), a(2), d(2), Q(2));
T23a = Transform(alpha(3), a(3), 0,    Q(3));
T23  = Transform(alpha(3), a(3), d(3), Q(3));
T34a = Transform(alpha(4), a(4), 0,    Q(4));
T34  = Transform(alpha(4), a(4), d(4), Q(4));
T45a = Transform(alpha(5), a(5), 0,    Q(5));
T45  = Transform(alpha(5), a(5), d(5), Q(5));

Origin = [0;0;0;1];

P0 = Origin;
P1 = T01a * Origin;
P2 = T01 * Origin;
P3 = T01*T12a * Origin;
P4 = T01*T12 * Origin;
P5 = T01*T12*T23a * Origin;
P6 = T01*T12*T23 * Origin;
P7 = T01*T12*T23*T34a * Origin;
P8 = T01*T12*T23*T34 * Origin;
P9  = T01*T12*T23*T34*T45a * Origin;
P10 = T01*T12*T23*T34*T45 * Origin;


X = [P0(1); P1(1); P2(1); P3(1); P4(1); P5(1); P6(1); P7(1); P8(1); P9(1); P10(1)];
Y = [P0(2); P1(2); P2(2); P3(2); P4(2); P5(2); P6(2); P7(2); P8(2); P9(2); P10(2)];
Z = [P0(3); P1(3); P2(3); P3(3); P4(3); P5(3); P6(3); P7(3); P8(3); P9(3); P10(3)];


Tip = T01*T12*T23*T34*T45*[0;0;0;1];
Tipx = T01*T12*T23*T34*T45*[10;0;0;1];
Tipy = T01*T12*T23*T34*T45*[0;10;0;1];
Tipz = T01*T12*T23*T34*T45*[0;0;10;1];

% draw the axis
clf;
plot([0, -100],[0, -100], 'r-');
hold on
plot([0, 150],[0, 0], 'r-');
plot([0, 0],  [0, 150], 'r-');

% draw the robot

plot(Y - 0.707*X,  Z - 0.707*X, 'b.-');
plot(TIP(2,:) - 0.707*TIP(1,:),  TIP(3,:) - 0.707*TIP(1,:), 'g');

% draw the tip axis
plot([Tip(2)-0.707*Tip(1), Tipx(2)-0.707*Tipx(1)],[Tip(3)-0.707*Tip(1),Tipx(3)-0.707*Tipx(1)],'g-');
plot([Tip(2)-0.707*Tip(1), Tipy(2)-0.707*Tipy(1)],[Tip(3)-0.707*Tip(1),Tipy(3)-0.707*Tipy(1)],'g-');
plot([Tip(2)-0.707*Tip(1), Tipz(2)-0.707*Tipz(1)],[Tip(3)-0.707*Tip(1),Tipz(3)-0.707*Tipz(1)],'g-');
 
pause(0.01);

end