ccl_rob_plot_arm

 ccl_rob_plot_arm (Q, L, C)
 A function to plot arm
 Input:
   Q               Joint space vector
   L               Link lengths
   C               Color

0001 function ccl_rob_plot_arm (Q, L, C)
0002 % ccl_rob_plot_arm (Q, L, C)
0003 % A function to plot arm
0004 % Input:
0005 %   Q               Joint space vector
0006 %   L               Link lengths
0007 %   C               Color
0008 if ~exist('C', 'var')
0009     C = 'r' ;
0010 end
0011 r1 = zeros(2,1); % base
0012 
0013 r2 = [L(1)*cos(Q(1));
0014     L(1)*sin(Q(1))];
0015 r3 = [r2(1) + L(2)*cos(Q(1)+Q(2));
0016     r2(2) + L(2)*sin(Q(1)+Q(2))];
0017 plot([r1(1) r2(1)], [r1(2) r2(2)], 'LineStyle', '-', 'Color', C ) ;
0018 plot([r2(1) r3(1)], [r2(2) r3(2)], 'LineStyle', '-', 'Color', C ) ;
0019 end