Enter Parameters
syntax and help ---------------------->

Two arguments must be passed into this program to extract data.

   1    parameters = (({{(Subscript[a, 1]), (Subscript[α, 1]), (Subscript[d, 1]), (Subscript[θ, 1])}, {(Subscript[a, 2]), (Subscript[α, 2]), (Subscript[d, 2]), (Subscript[θ, 2])}, {(Subscript[a, 3]), (Subscript[α, 3]), (Subscript[d, 3]), (Subscript[θ, 3])}}))

	Every  column  ({{(Subscript[a, 1]), (Subscript[α, 1]), (Subscript[d, 1]), (Subscript[θ, 1])}})   represents one joint on the manipulator in this case their are 3 joints. 
  (Subscript[a, i])=\(Distance\ between\ the\ origins\ of\ two\ neighboring\ links\ along\ the\ x\ direction\)
         \(α\_1 = \ 
          angle\ between\ the\ axis\ Z\_\(i - 1\)\ and\ Z\_i\ taken\ about\ the\ x - axis\), \(and\ is\ positive\ \ counter\ clockwise\ direction . 
\ \ d\_1 = \ \(Distance\ between\ the\ two\ origins\ in\ the\ z\ direction . \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
θ\_1 = \ 
            Angle\ between\ the\ x\_\(i - 1\)\ and\ the\ x\_i\ axis\ about\ the\ z\_\(i - 1\)\ axis\)\), Counter clockwise is positive.
	
2    The required input type  is a list of variables r and t. 
        One variable must be entered for each joint  on the \(manipulator . 
\ \ \ \ \ \ \ \ Enter\) r for Rotational joint, or t  for translational.

3  The other parameter you need to enter is you want to calculate the inertia on the joints
	  \(is\ \ a\ list\ of\ the\ various\ types\ of\ inertias\ an\ example\ is\ shown\ below . \ 
          you\ may\ need\ to\ add\ more\ inertia\ parameters\) 
	  (i only gave 3) for your application.

Input Types
		`C  -  Center
		`mid - Center
		`S  -  Simplified
		 `T -  Transposed
	R[
            q] -- transformation with respect to a give frame
		Rmid[q]
		TR[q]
		TRmid[q]
	\(\(P[
            q]--\)\ Position\ \ at\ the\ end\ of\ the\ link\ with\ respect\ to\ the\ base\ frame\)
		TP[q]
		Pmid[q]
		TPmid[q]
	B[q]-- 
        Inertia matrix (use q = 
            1 for typical calculations)
		BS[q]
		BC[q]
		BCS[q]
	Jp[q]-- Positional Jacobian
		TJp[q]
		TCJp[q]
		CJp[q]
	Jo[q]-- 
        Orientational Jacobian transform
		TJo[q]
		TCJo[q]
		CJo[q]

parameters = (({{0, ((-π)/2), 300, (Subscript[θ, 1])}, {0, ((-π)/2), 0, (Subscript[θ, 2])}, {0, 0, (Subscript[d, 3]), 0}}));type = {r, r, t};Inertia = {solidcylinder[(Subscript[m, 1]), (Subscript[l, 1]), (Subscript[r, 1])], 
          hollowcylinder[(Subscript[m, 2]), (Subscript[l, 2]), (Subscript[R, 2]), (Subscript[r, 2])], thinrod[(Subscript[m, 3]), (Subscript[l, 3])]};

Primary Algorithm **Re-evaluate this cell if you change your parameters

Denavit[val_, lentemp_]:= 

(len = Length[lentemp];
h  =  Table[0, {i, len}];
  
For[
i = 1,i ≤ len,

h[[i]]= (({{Cos[
                              val[[i, 4]]], -Sin[val[[i, 4]]]*
                              Cos[val[[i, 2]]], Sin[val[[i, 4]]]*
                              Sin[val[[i, 2]]], val[[i, 1]] Cos[
                                val[[i, 4]]]}, {Sin[val[[i, 4]]], Cos[val[[i, 4]]]*
                              Cos[val[[i, 2]]], -Cos[
                                  val[[i, 4]]]*
                              Sin[val[[i, 2]]], val[[i, 1]] Sin[
                                val[[i, 4]]]}, {0, Sin[val[[i, 2]]], Cos[
                              val[[i, 2]]], val[[i, 3]]}, {0, 0, 0, 1}}))
;i++]

)

Denavit[parameters, type]

Individual Function Definitions

Piecewise Function Definitions for extracting positional and directional parameters

R[temp_] := (
  
Dot @@ Table[h[[i]], {i, 1, temp}])

TR[temp_] := (
  
Transpose[
          Dot @@ Table[h[[i]], {i, 1, temp}]])

Rmid[
        temp_] := (
  
Dot @@ 
          Table[h[[i]] /. (Subscript[d, i]) -> ((Subscript[d, i]) - ((Subscript[l, i])/2)), {i, 1, 
              temp}])

P[temp_] := (
   
Take[R[temp][[All, 4]], 3])

Pmid[
        temp_] := (
  
Take[Rmid[temp][[All, 4]], 3] /. 
          (Subscript[d, i]) -> ((Subscript[d, i]) - ((Subscript[l, i])/2)))

Zmid[
        temp_] := (
  
Take[Rmid[temp][[All, 3]], 3])

Z[temp_] := (
  
Take[R[
              temp][[All, 3]], 3])

Definitions for computation of the Jacobian transformation matrix

Positional

Jp[temp_] := (
  
Module[{ff, tt}, 
ff = 
            Length[type]; 
tt  =  
            Transpose[
              PadRight[
                Table[If[type[[j]] == r, 
                    Z[j]\[Cross](P[temp] - P[j]), , Z[j - 1]], {j, 1, 
                    temp}], {3, 3}]]])

TCJp[
        temp_] := (
  
Module[{ff, 
            tt}, 
ff = 
            Length[type]; 
tt  =  
            PadRight[
              Table[If[type[[j]] == r, 
                  Z[j]\[Cross](Pmid[temp] - Pmid[j]), , Z[j - 1]], {j, 1, 
                  temp}], {3, 3}]])

CJp[
        temp_] := (
  
Module[{ff, 
            tt}, 
ff = 
            Length[type]; 
tt  =  
            Transpose[
              PadRight[
                Table[If[type[[j]] == r, 
                    Z[j]\[Cross](Pmid[temp] - Pmid[j]), , Z[j - 1]], {j, 
                    1, temp}], {3, 3}]]])

TJp[
        temp_] := (
  
Module[{ff, 
            tt}, 
ff = 
            Length[type]; 
tt  =  
            PadRight[
              Table[If[type[[j]] == r, 
                  Z[j]\[Cross](P[temp] - P[j]), , Z[j - 1]], {j, 1, 
                  temp}], {3, 3}]])

Orientational

TCJo[
        temp_] := (  Module[{ff, tt}, 
          ff = Length[type]; 
tt = 
            PadRight[
              Table[If[type[[j]] == r, Zmid[j], , {0, 0, 0}], {j, 
                  1, temp}], {3, 3}]])

CJo[
        temp_] := (  Module[{ff, tt}, 
          ff = Length[type]; 
tt = 
            PadRight[
              Transpose[
                Table[If[type[[j]] == r, Zmid[j], , {0, 0, 0}], {j, 
                    1, temp}]], {3, 3}]])

TJo[
        temp_] := (
  
Module[{ff, 
            tt}, 
ff = 
            Length[type]; 
tt = 
            PadRight[
              Table[If[type[[j]] == r, Z[j], , {0, 0, 0}], {j, 1, 
                  temp}], {3, 3}]])

Jo[temp_] := (  Module[{ff, tt}, 
          ff = Length[type]; 
tt = 
            PadRight[
              Transpose[
                Table[If[type[[j]] == r, Z[j], , {0, 0, 0}], {j, 1, 
                    temp}]], {3, 3}]])

Primary Algorithms

(* 
      Pimary Algorithm For calculating the Omnidirectional inertia Of a Specific link 
With the center of mass at the center of the link *) 
BC[
        j_] := (
(∑_(i = 1)^(Length[type] - j + 1))(Subscript[m, i])*(TCJp[i] . 
                  CJp[i]) + (∑_(i = 1)^(Length[type] - j + 1))Inertia[[i]]*TCJo[i] . CJo[i])

(* 
      Pimary Algorithm For calculating the Omnidirectional inertia Of a Specific link 
With the center of mass at the center of the link *) 
 (* 
      Simplified Version *) 
BCS[
        j_] := (
(∑_(i = 1)^(Length[type] - j + 1))(Subscript[m, i])*(TCJp[i] . 
                  CJp[i]) + (∑_(i = 1)^(Length[type] - j + 1))Inertia[[i]]*TCJo[i] . CJo[i])

(* 
      Pimary Algorithm For calculating the Omnidirectional inertia Of a Specific link 
with the center of mass at the end of the link *) 
B[
        j_] := (
(∑_(i = 1)^(Length[type] - j + 1))(Subscript[m, i])*(TJp[i] . 
                  Jp[i]) + (∑_(i = 1)^(Length[type] - j + 1))Inertia[[i]]*TJo[i] . Jo[i])

(* 
      Pimary Algorithm For calculating the Omnidirectional inertia Of a Specific link 
with the center of mass at the end of the link 
Simplified Version *) 
BS[
        j_] := (
(∑_(i = 1)^(Length[type] - j + 1))(Subscript[m, i])*(TJp[i] . 
                  Jp[i]) + (∑_(i = 1)^(Length[type] - j + 1))Inertia[[i]] . Jo[i])

Definitions for various moments of inertia you will probably need add more to accomodate.

Thin Rod

thinrod[mass_, 
        length_] := (
(1/3) mass*
          (length^2)
)

Hollow Cylinder

hollowcylinder[mass_, length_, R_, 
        r_] := (
mass*(((R^2)/4) + ((r^2)/4) + 
              ((length^2)/3))
)

SolidCylinder

solidcylinder[mass_, length_, 
        radius_] := (
(1/4) mass*(radius^2) + (1/12) 
            mass*(length^2)
)