%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Drawing of a PUMA 560
%
% Inspired by Figure 4.4 in Craig (1989)
%
% Author: Kjell Magne Fauske
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (c) 2009 Kjell Magne Fauske
% This code is released under the MIT license
% See http://www.opensource.org/licenses/mit-license.php for details. 


%
% Define some basic constants
%
def O (0,0,0) % Origo
def I [1,0,0] % X
def J [0,1,0] % Y
def K [0,0,1] % Z


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Define some basic geometric shapes like cubes and cylinders. 
% All the shapes have unit sizes and are ment to be scaled when used.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

def jR 1
def jR_inner 0.5/2
def jH 2/2
def cylinder put{rotate(90,[1,0,0])}{
    def n 20   
  sweep[fill=lightgray,  draw=gray]{n<>, rotate(360/n,[0,1,0])}
    line[fill=gray](jR,0)(jR,jH)
}

% A unit cube with center (0,0,0)
def box put{translate([-1.5,-0.5,0])}{
    def n 1   
  sweep[fill=lightgray,  draw=gray]
        {n,translate([0,n,0])}
    sweep[fill=lightgray,  draw=gray]
        {n, translate([n,0,0])}line[fill=gray](1,0,0)(1,0,1)
}

def boxd put{translate([-1.5,-0.5,0])}{
    def n 1   
  sweep[fill=darkgray,  draw=gray]
    {n,translate([0,n,0])}
    sweep[fill=darkgray,  draw=gray]
        {n, translate([n,0,0])}line[fill=darkgray](1,0,0)(1,0,1)
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

def sz 8 % lengden av aksene
def axes {
    line [arrows=<->, draw=blue, line width=1.5pt] (sz,0,0)(O)(0,sz,0)
    line [arrows=->,draw=blue, line width=1.5pt]  (O)(0,0,sz)
    line [arrows=<->,draw=blue, line width=.5pt, line style=dotted, lay=over] (sz,0,0)(O)(0,sz,0)
    line [draw=blue, line width=.5pt, line style=dotted, lay=over]  (O)(0,0,sz)
    
}

%
% Draw a basic PUMA 560 linkshape
%
def linkshape {
    % First we draw the top plate
    def linkplate put{translate([1,0,0])}{
        polygon[fill=lightgray](-3,0)(-3,1)(-2,2)(0,2)(4,1)(5,0.5)(5,0)
                             (5,-0.5)(5,-0.5)(4,-1)(0,-2)(-2,-2)(-3,-1)
    }
    % Extrude along the z-azis
    def n 1   
  sweep[cull=false, fill=gray,  draw=darkgray]
        {n,translate([0,0,n])}{linkplate}
  special|\fill #1 circle (2pt);|(0,0,1)
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Define size and shape of robot
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% joint 0
def j0_r 2
def j0_h 1
def j0_theta 0

% link 0
def l0_r 1
def l0_h 4

% joint 1
def j1_r 1
def j1_h 2.5
def j1_theta 45

% link 1
def l1_sx 1.5
def l1_sy 0.9
def l1_l 5*l1_sx

% joint 2
def j2_r 0.7
def j2_h 2
def j2_theta -45

%link 2
def l2_sx l1_sx*0.7
def l2_sy l1_sy*0.7
def l2_l 6*l2_sx


def joint0 {
    put {scale([j0_r, j0_r, j0_h])}{cylinder}
}

def link0 {
    put {translate([0,0,j0_h])}{
        put {scale([l0_r, l0_r, l0_h])}{cylinder}
    }
}

def joint1 {
    put {rotate(90,[1,0,0]) then translate([0,j1_h/2,j0_h+l0_h+j1_r]) then rotate(j0_theta,[0,0,1])}{
        put {scale([j1_r, j1_r, j1_h])}{cylinder}
    }
}


def T01 rotate(90,[1,0,0]) then translate([0,j1_h,j0_h+l0_h+j1_r]) then rotate(j0_theta,[0,0,1])
def T02 rotate(j1_theta,[0,0,1]) then [[T01]]

def link1 {
    put {([[T02]])}{put{scale([l1_sx,l1_sy,1])}{linkshape}}
}


def T03 translate([l1_l,0,0]) then [[T02]]
def joint2 {
    put {[[T03]]}{put {scale([j2_r, j2_r, j2_h])}{cylinder}} 
}

def T04 translate([0,0,j2_h]) then rotate(j2_theta,[0,0,1]) then [[T03]]
def link2 {
    put {[[T04]]}{
        put{scale([l2_sx,l2_sy,1])}{linkshape}
    %   put{translate([l2_l,0,0])}{box}
    }
}


def g_l 2 def g_w 1 def g_h 0.8
def grip {
    put {scale([g_l, g_w,g_h])}{box}
    put {translate([-g_l*4/3,0,g_h]) then scale([0.1*g_l, g_w,1])}{boxd}
    put {translate([g_l*4/3,0,g_h]) then scale([0.1*g_l, g_w,1])}{boxd}
}

def T05 translate([l2_l,0,0]) then [[T04]]
def joint3 {
    put {[[T05]]}{
        {box}
        put{translate([-(g_l/2)/2,0,0]) then rotate(90,[0,1,0]) then rotate(0,[1,0,0])  }{
        {grip}
        }
    }
    
}


    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define how to look at our work of art
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def viewpoint (4,-14,10)
def lookat (0, 0, 0)
% Set upvector along the z-axis
def upvector [0,0,1]


put{ view((viewpoint),(lookat), [upvector])} {
    {axes}
    {joint0}{link0}{joint1}{link1}{joint2}{link2}{joint3}
    %{grip}
}

%global {
%  set [line width=.3pt,scale=0.3]
%}

global { language tikz }