%
% An illustration of the Denavit-Hartenberg parameters used in robotics
%
% Inspired by several figures in the book "Inroduction to Robotics: Mechanics
% and Control" by John J. Craig (1989)
%
% Author: Kjell Magne Fauske

% Note: This is one of the first illustration I have made with Sketch. The
%   code could definitely be more elgant

%
% Define the Denavit-Hartenberg parameters for use in the figure
%
def a_im 4 % a_{i-1}
def d_i 2  
def alpha_im 20 % alpha_{i-1}
def theta_i 30% 
def a_i 4
def alpha_i 20

% avstand fra ledd til koordinatsystem
def dZ 4

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Define some basic constants
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

def O (0,0,0) % Origo

% The ee constant is used for tweaking the hidden surface removal algorithm
% in Sketch. The constant is too small to give visible results, but affects
% which surfaces that are drawn. 
def ee 0.0001


% 
% Draw the axes of the current coordinate system
%
def sz 2 % length of each axis
def axes {
    line [arrows=<->, linecolor=blue, linewidth=1.5pt] (sz,0,0)(O)(0,sz,0)
    line [arrows=->,linecolor=blue, linewidth=1.5pt]  (O)(0,0,sz)
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Code for drawing the solid objects.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% The code for drawing the solid joints and links is not pretty. 
% My intention was to use the Denavit-Hartenberg transformations, but 
% its not 'physically' possible to translate the link along the joint axes
% without changing the joint angles.

% Draw a single joint
def jR 1.5/2
def jR_inner 0.5/2
def jH 2/2
def joint put{rotate(90,[1,0,0])}{
	def n 20   
  sweep[fillcolor=lightgray, cull=false, linecolor=gray]{n<>, rotate(360/n,[0,1,0])}
  	line[fillcolor=gray](jR,-jH/2)(jR,jH/2)
  sweep[fillcolor=darkgray, cull=false]{n<>, rotate(360/n,[0,1,0])}
  	line(jR_inner,-jH/2-ee)(jR_inner,jH/2+ee)
}

% Draw a link
def lW 1/2
def lH 0.5/2
def link put{rotate(90,[-1,0,0]) then translate([-a_im/2,jR*(2/3),0])}{
	def n_segs 20
	def trans rotate(180 / n_segs, [0,-1,0])  then translate(((d_i*cos(alpha_im))/n_segs)*[0,-1,0])
	def dR a_im/2+lW/2 
  sweep[cull=false, fillcolor=lightgray,linecolor=gray] 
  	{ n_segs, [[trans]]} line (dR,-lH/2)(dR,lH/2)
  sweep[cull=false,fillcolor=lightgray, linecolor=gray] 
  	{ n_segs, [[trans]]} line (dR-lW,-lH/2)(dR-lW,lH/2)
  sweep[cull=false, fillcolor=lightgray, linecolor=gray] 
  	{ n_segs, [[trans]]}	line[linecolor=red] (dR-lW,lH/2+ee)(dR,lH/2+ee)
  sweep[cull=false,fillcolor=lightgray, linecolor=gray] 
  	{ n_segs, [[trans]]}	line[linecolor=red] (dR-lW,-lH/2-ee)(dR,-lH/2-ee)
}


% Combine both joints and link. 
% I had to do some quick hacks to make it look good. Will probably look
% weird if you change the parameters or the viewpoint. 
def jointslink {			  	put{rotate(alpha_im,[0,0,-1])}{{put{scale([1+(1-cos(alpha_im)),1,1])}{link}{joint}}}
	put{rotate(180,[0,0,1]) then translate([0,0,-dZ])}{
			put {translate([a_im,0,0]) 
			     then rotate(alpha_im,[1,0,0])}{ 
				put{ translate([0,0,d_i+dZ]) }{joint}
			}
	}
}

% I had to do some more cheating for the second link :(
def jointslink2 {
  put{rotate(alpha_i+5,[0,0,-1])}{{put{scale([(1+(1-cos(alpha_i)))*1.1,1,1])}{link}{joint}}}
	put{rotate(180,[0,0,1]) then translate([0,0,-dZ-jH])}{
			put {translate([a_i,0,0]) 
			     then rotate(alpha_i,[1,0,0])}{ 
				put{ translate([0,0,dZ+jH+d_i+0.2]) }{joint}
			}
	}
}



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Main drawing
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

def dv 0.5
def drawillustration {		
	% Start by drawing the first solid link shape
	{jointslink}
	% Translate -dZ along the z-axis, then rotate about the z-axis so that our
	% x_{i-1} axis points in the right direction 
	put {rotate(180,[0,0,1]) then translate([0,0,-dZ])}{
		% Draw the long z_{i-1}-axis with annotations
		line[linecolor=red] (0,0,6)(0,0,-4)
		special |\uput[u]#1{Axis $i-1$}|(0,0,6)
		
		% Draw the line connection the z_{i-1} and z_i axis
		line (O)(a_im,0,0)
		% draw a 'square' to indicate a right angle
		line (dv,0,0)(dv,0,-dv)(0,0,-dv)
		% label
		special |\uput[d]#1{$a_{i-1}$}|(a_im/2,0,0)
		
		% Draw the coordinate axes of the z_{i-1} system with labels
		{axes}
		special |\uput[u]#1{$x_{i-1}$}\uput[u]#2{$y_{i-1}$}\uput[l]#3{$z_{i-1}$}|
    (sz,0,0)(0,sz,0)(0,0,sz)
		special |\psdot#1|(O)
		
		% Draw the alpha_{i-1} angle with label. Sketch currently does not support
		% curves. Use a pscurve instead, with appropiate computed startpoint midpoint
		% and endpoint. 
		def dr 3 % distance to the  alpha_{i-1} label
		special |\pscurve[arrows=->]#1#2#3\uput[dr]#2{$\alpha_{i-1}$}|
			(0,0,-dr)
			(0,dr*sin(alpha_im/2),-dr*cos(alpha_im/2))
			(0,dr*sin(alpha_im),-dr*cos(alpha_im))
	 
		% We are now almost finished with the first joint axis. Translate to the z_i axis
		put {translate([a_im,0,0]) then rotate(alpha_im,[1,0,0]) }{
			% translate back along the x_{i-1} axis to draw the dashed line
			% in the alpha_{i-1} annotation.
			put {translate([-a_im,0,0])}{line[linestyle=dashed] (O)(0,0,-3.5)}
			
			% Now draw the z_i joint axis
			line[linecolor=red] (0,0,10)(0,0,-5)
			special |\uput[u]#1{Axis $i$}|(0,0,10)
			% draw a right angle
			line (-dv,0,0)(-dv,0,-dv)(0,0,-dv)
			
			% Draw theta_i annotations. Use the same method as above.
			% extend the x_{i-1} axis
			line[linestyle=dashed] (0,0,0)(3,0,0)
			def dr 2
			special |\pscurve[arrows=->]#1#2#3\uput[r]#2{$\theta_i$}|
				(dr,0,0)
				(dr*cos(theta_i/2),dr*sin(theta_i/2),0)
				(dr*cos(theta_i),dr*sin(theta_i),0)
			
			put{rotate(theta_i,[0,0,1])}{
				% draw the final dashed line to complete the theta_i annotation
				line[linestyle=dashed] (0,0,0)(3,0,0)
				
			
				
				% draw the d_i label
				special |\uput[l]#1{$d_i$}|(0,0,d_i/2)
				
				% Now its time to draw the z_i coordinate system. Translate d_i along the
				% z_i axis first
				put{translate([0,0,d_i])}{
					% Draw the coordinate axes of the z_{i-1} system with labels
					special |\psdot#1|(O)
					{axes}
					special |\uput[ul]#1{$x_i$}\uput[l]#2{$y_i$}\uput[r]#3{$z_i$}|
    				(sz,0,0)(0,sz,0)(0,0,sz)
    			line (O)(a_i,0,0)
    			
    			special |\uput[d]#1{$a_{i}$}|(a_i/2,0,0)
					put{translate([0,0,dZ+jH]) then rotate(180,[0,0,1])}{jointslink2}
					
					% Finally we draw the i+1 joint axis
					put {translate([a_i,0,0]) then rotate(alpha_i,[1,0,0])}{
						line[linecolor=red] (0,0,10)(0,0,-10)
						line (-dv,0,0)(-dv,0,-dv)(0,0,-dv)
						special |\uput[u]#1{Axis $i+1$}|(0,0,10)
					}
				}
			}
		}
	}
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define how we want to look at the drawing.
%
% Experiment with different viewpoints.
% Note: If you change the viewpoint, you may have to manipulate the location 
% of the labels to achieve good results. 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%def viewpoint (-14,14,6)
def viewpoint (-4,14,6)
def lookat (0, 0, 0)
% Define the global z-axis to point 'upwards'. The default z-axis points
% out of the page.
def upvector [0,0,1]

put{ view((viewpoint),(lookat), [upvector])} {
	{drawillustration}
}