% pen down
[P1, T1] = MoveTo([0; 80; 20], [0; 80; 0], 1);
% draw a box
[P2, T2] = MoveTo(P1, [-30; 80; 0], 2);
[P3, T3] = MoveTo(P2, [-30; 20; 0], 2);
[P4, T4] = MoveTo(P3, [30; 20; 0], 2);
[P5, T5] = MoveTo(P4, [30; 80; 0], 2);
[P6, T6] = MoveTo(P5, [0; 80; 0], 2);
% draw a circle
[P7, T7] = CircleXY(P6, [0; 50; 0], 4);
% draw arcs
[P8, T8] = ArcXY(P7, [-9; 70; 0], [-8; 58; 0], 1);
% draw a circle
[P9, T9] = CircleXY(P8, [0; 50; 0], 2);
% draw arcs
[P10, T10] = ArcXY(P9, [-20; 58; 0], [-30; 50; 0], 1);
[P11, T11] = ArcXY(P10, [-20; 40; 0], [ -8; 42; 0], 1);
[P12, T12] = ArcXY(P11, [-10; 30; 0], [  0; 20; 0], 1);
[P13, T13] = ArcXY(P12, [ 10; 30; 0], [  8; 42; 0], 1);
[P14, T14] = ArcXY(P13, [ 20; 40; 0], [ 30; 50; 0], 1);
[P15, T15] = ArcXY(P14, [ 20; 60; 0], [  8; 58; 0], 1);
[P16, T16] = ArcXY(P15, [ 10; 70; 0], [  0; 80; 0], 1);
% pen up
[P17, T17] = MoveTo(P16, [0; 80; 20], 1);

TIP = [T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17];

Q = [];
npt = length(TIP)
for i=1:npt
   Qi = InverseRRR(TIP(:,i));
   Pi = RRR(Qi, TIP);
   Q = [Q, Qi];
   end