Whole_Body_Kinematics_of_NAO_1.nb

(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 10.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[     94564,       2678]
NotebookOptionsPosition[     91042,       2552]
NotebookOutlinePosition[     91414,       2568]
CellTagsIndexPosition[     91371,       2565]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell["Forward Kinematics of NAO", "Title",
 CellChangeTimes->{{3.677838513281303*^9, 3.6778385228008475`*^9}, {
  3.677838567220388*^9, 3.6778385962090464`*^9}}],

Cell[CellGroupData[{

Cell["Head", "Section",
 CellChangeTimes->{{3.6778359932461653`*^9, 3.67783603982883*^9}}],

Cell[BoxData[{
 RowBox[{"DHPHead", ":=", 
  RowBox[{"(", GridBox[{
     {"aa", "b", "c", "\[Theta]"},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      SubscriptBox["\[Theta]HD", "1"]},
     {"0", "0", "0", 
      SubscriptBox["\[Theta]HD", "2"]}
    }], ")"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]HD", "1"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]HD", "2"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "3"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"q", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{"Cos", "[", 
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Cos", "[", 
            RowBox[{"DHPHead", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"Sin", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"Sin", "[", 
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Sin", "[", 
            RowBox[{"DHPHead", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {"0", 
         RowBox[{"Sin", "[", 
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
         RowBox[{"Cos", "[", 
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "3"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"a", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{
          RowBox[{"DHPHead", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPHead", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"DHPHead", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"i", "+", "1"}], ",", "2"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"QHD3to1", "=", 
   RowBox[{
    RowBox[{"q", "[", "1", "]"}], ".", 
    RowBox[{"q", "[", "2", "]"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"PHD3to1", "=", 
   RowBox[{
    RowBox[{"a", "[", "1", "]"}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"a", "[", "2", "]"}]}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"THD3to1", "=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PHD3to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.677836067707424*^9, 3.677836303957937*^9}, {
   3.6778379231735506`*^9, 3.677837951299159*^9}, {3.6778380588223095`*^9, 
   3.677838109921232*^9}, {3.6778381559638653`*^9, 3.6778384013819027`*^9}, {
   3.6778384585251713`*^9, 3.677838471668923*^9}, 3.6778393920605664`*^9, {
   3.677841648045601*^9, 3.6778416556170344`*^9}, {3.6778418694582653`*^9, 
   3.6778418996229906`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"THD3to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677836067707424*^9, 3.677836303957937*^9}, {
   3.6778379231735506`*^9, 3.677837951299159*^9}, {3.6778380588223095`*^9, 
   3.677838109921232*^9}, {3.6778381559638653`*^9, 3.6778384013819027`*^9}, {
   3.6778384585251713`*^9, 3.677838471668923*^9}, {3.6778393920605664`*^9, 
   3.677839417144001*^9}, 3.6778416566980963`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", "0"},
     {"0", "0", 
      RowBox[{"-", "1"}], "0"},
     {"0", "1", "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394426884623`*^9, 3.6778394736692343`*^9, 3.67783985438301*^9, 
   3.6778401281796703`*^9, 3.6778402879348073`*^9, 3.6778403218257456`*^9, 
   3.6778406044799128`*^9, 3.677840835106104*^9, 3.677840956810065*^9, 
   3.6778411104388523`*^9, 3.6778416323527036`*^9, 3.6778417286122093`*^9, {
   3.6778419085925035`*^9, 3.6778419332909164`*^9}, 3.677841998301635*^9, 
   3.6778448262073812`*^9, 3.6778476942764263`*^9, 3.6778477373898916`*^9, 
   3.6778490119157906`*^9, 3.677849047905849*^9, 3.6778491312156143`*^9, {
   3.6778491873448243`*^9, 3.6778492079730043`*^9}, 3.677849610425023*^9, 
   3.677849944392125*^9, 3.677849997745177*^9, 3.677850130064745*^9, 
   3.67785051933801*^9, {3.677850622280898*^9, 3.6778506368687325`*^9}, {
   3.677850715484229*^9, 3.6778507331732407`*^9}, {3.677850947979527*^9, 
   3.677851004801777*^9}, 3.6778513367827654`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"QHD3to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677836067707424*^9, 3.677836303957937*^9}, {
   3.6778379231735506`*^9, 3.677837951299159*^9}, {3.6778380588223095`*^9, 
   3.677838109921232*^9}, {3.6778381559638653`*^9, 3.6778384013819027`*^9}, {
   3.6778384585251713`*^9, 3.677838471668923*^9}, {3.6778393920605664`*^9, 
   3.677839421432246*^9}, 3.6778416581281776`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0"},
     {"0", "0", 
      RowBox[{"-", "1"}]},
     {"0", "1", "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394427034626`*^9, 3.6778394736842346`*^9, 3.6778398543990107`*^9, 
   3.6778401281966705`*^9, 3.677840287968809*^9, 3.677840321838746*^9, 
   3.6778406044949136`*^9, 3.6778408351221046`*^9, 3.6778409568220654`*^9, 
   3.677841110451853*^9, 3.6778416323677044`*^9, 3.67784172862721*^9, {
   3.6778419086065044`*^9, 3.6778419333059173`*^9}, 3.6778419983156357`*^9, 
   3.677844826221382*^9, 3.677847694296427*^9, 3.6778477374088926`*^9, 
   3.677849011928791*^9, 3.67784904791885*^9, 3.677849131227615*^9, {
   3.677849187356825*^9, 3.677849207984005*^9}, 3.6778496104350243`*^9, 
   3.6778499444021254`*^9, 3.6778499977561774`*^9, 3.6778501300757456`*^9, 
   3.677850519348011*^9, {3.677850622290899*^9, 3.677850636878733*^9}, {
   3.6778507154962296`*^9, 3.6778507331832414`*^9}, {3.6778509479905276`*^9, 
   3.6778510048127775`*^9}, 3.6778513368107667`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PHD3to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677836067707424*^9, 3.677836303957937*^9}, {
   3.6778379231735506`*^9, 3.677837951299159*^9}, {3.6778380588223095`*^9, 
   3.677838109921232*^9}, {3.6778381559638653`*^9, 3.6778384013819027`*^9}, {
   3.6778384585251713`*^9, 3.677838471668923*^9}, {3.6778393920605664`*^9, 
   3.6778394247894382`*^9}, 3.6778416594072514`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0"},
     {"0"},
     {"0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677839442718464*^9, 3.677839473700236*^9, 3.6778398544130116`*^9, 
   3.6778401282106714`*^9, 3.6778402879978113`*^9, 3.6778403218517475`*^9, 
   3.6778406045079145`*^9, 3.677840835135105*^9, 3.677840956842067*^9, 
   3.6778411104648533`*^9, 3.677841632380705*^9, 3.6778417286392107`*^9, {
   3.677841908619505*^9, 3.6778419333199177`*^9}, 3.677841998328636*^9, 
   3.677844826234383*^9, 3.677847694313428*^9, 3.677847737425894*^9, 
   3.677849011940792*^9, 3.6778490479308505`*^9, 3.677849131237615*^9, {
   3.6778491873678255`*^9, 3.6778492079960055`*^9}, 3.6778496104450245`*^9, 
   3.677849944411126*^9, 3.6778499977721786`*^9, 3.6778501300857463`*^9, 
   3.6778505193580112`*^9, {3.6778506223008995`*^9, 3.6778506368897333`*^9}, {
   3.6778507155052304`*^9, 3.677850733193242*^9}, {3.677850947999528*^9, 
   3.6778510048227787`*^9}, 3.6778513368227673`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"THD1toTorso", ":=", 
   RowBox[{"(", GridBox[{
      {"1", "0", "0", "0"},
      {"0", "0", "1", "0"},
      {"0", 
       RowBox[{"-", "1"}], "0", "126.5"},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"THD3toTorso", ":=", 
   RowBox[{"THD1toTorso", ".", "THD3to1"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"THD3toTorso", "//", "MatrixForm"}]}], "Input",
 CellChangeTimes->{{3.677836067707424*^9, 3.677836303957937*^9}, {
   3.6778379231735506`*^9, 3.677837951299159*^9}, {3.6778380588223095`*^9, 
   3.677838109921232*^9}, {3.6778381559638653`*^9, 3.6778384013819027`*^9}, {
   3.6778384585251713`*^9, 3.677838471668923*^9}, {3.6778393920605664`*^9, 
   3.6778394247894382`*^9}, 3.6778416621774096`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1.`", "0.`", "0.`", "0.`"},
     {"0.`", "1.`", "0.`", "0.`"},
     {"0.`", "0.`", "1.`", "126.5`"},
     {"0.`", "0.`", "0.`", "1.`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394427314644`*^9, 3.6778394737152367`*^9, 3.677839854427012*^9, 
   3.6778401282246723`*^9, 3.6778402880268126`*^9, 3.6778403218677483`*^9, 
   3.677840604522915*^9, 3.677840835148106*^9, 3.677840956864068*^9, 
   3.677841110479854*^9, 3.677841632396706*^9, 3.677841728656212*^9, {
   3.677841908634506*^9, 3.677841933334919*^9}, 3.677841998342637*^9, 
   3.677844826248384*^9, 3.677847694331429*^9, 3.677847737442895*^9, 
   3.6778490119537926`*^9, 3.677849047944851*^9, 3.6778491312496157`*^9, {
   3.677849187378826*^9, 3.677849208008006*^9}, 3.6778496104570255`*^9, 
   3.677849944422127*^9, 3.677849997783179*^9, 3.6778501300967464`*^9, 
   3.677850519369012*^9, {3.6778506223129*^9, 3.6778506369017344`*^9}, {
   3.6778507155162306`*^9, 3.6778507332042427`*^9}, {3.6778509480105286`*^9, 
   3.677851004834779*^9}, 3.6778513368377686`*^9}]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["Lef Hand", "Section",
 CellChangeTimes->{{3.677838991165636*^9, 3.677839005669466*^9}}],

Cell[BoxData[{
 RowBox[{"DHPLH", ":=", 
  RowBox[{"(", GridBox[{
     {"aa", "b", "c", "\[Theta]"},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      SubscriptBox["\[Theta]LH", "1"]},
     {
      SubscriptBox["lh", "1"], "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LH", "2"], "+", 
       FractionBox["\[Pi]", "2"]}]},
     {"0", 
      SubscriptBox["lh", "2"], 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LH", "3"], "+", "\[Pi]"}]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LH", "4"], "+", "\[Pi]"}]},
     {"0", 
      SubscriptBox["lh", "3"], "0", 
      SubscriptBox["\[Theta]LH", "5"]}
    }], ")"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LH", "1"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LH", "2"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LH", "3"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LH", "4"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]LH", "5"], ":=", "0"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["lh", "1"], "=", "15"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["lh", "2"], "=", "105"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["lh", "3"], "=", "55.95"}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["lh", "1"], "=", "0.015"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["lh", "2"], "=", "0.105"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["lh", "3"], "=", "0.05595"}], ";"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "6"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"q", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{"Cos", "[", 
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Cos", "[", 
            RowBox[{"DHPLH", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"Sin", "[", 
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Sin", "[", 
            RowBox[{"DHPLH", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {"0", 
         RowBox[{"Sin", "[", 
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
         RowBox[{"Cos", "[", 
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "6"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"a", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{
          RowBox[{"DHPLH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"DHPLH", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"i", "+", "1"}], ",", "2"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"QLH6to1", "=", 
   RowBox[{
    RowBox[{"q", "[", "1", "]"}], ".", 
    RowBox[{"q", "[", "2", "]"}], ".", 
    RowBox[{"q", "[", "3", "]"}], ".", 
    RowBox[{"q", "[", "4", "]"}], ".", 
    RowBox[{"q", "[", "5", "]"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"PLH6to1", "=", 
   RowBox[{
    RowBox[{"a", "[", "1", "]"}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"a", "[", "2", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"a", "[", "3", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"a", "[", "4", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"a", "[", "5", "]"}]}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"TLH6to1", "=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.6778391135066338`*^9, {3.677839144077382*^9, 3.6778392771469936`*^9}, {
   3.6778393133960667`*^9, 3.6778393612808056`*^9}, {3.6778396791449866`*^9, 
   3.6778396928347692`*^9}, {3.6778410073159533`*^9, 3.6778410226418304`*^9}, 
   3.6778416221771216`*^9, {3.677841838929519*^9, 3.677841856528526*^9}, 
   3.6778419882970624`*^9, {3.6778453877955027`*^9, 3.6778454119508843`*^9}, {
   3.6778499862535195`*^9, 3.6778499881596284`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TLH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.6778391135066338`*^9, {3.677839144077382*^9, 3.6778392771469936`*^9}, {
   3.6778393133960667`*^9, 3.6778393612808056`*^9}, 3.6778416233481884`*^9, {
   3.6778453851183496`*^9, 3.677845385182353*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1", "160.95`"},
     {"0", 
      RowBox[{"-", "1"}], "0", "0.`"},
     {"1", "0", "0", "15.`"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394427844677`*^9, 3.6778394737672396`*^9, 3.677839854469014*^9, 
   3.6778401282916765`*^9, 3.677840288112818*^9, 3.677840321935752*^9, 
   3.677840604577918*^9, 3.6778408352091093`*^9, 3.677840956921071*^9, 
   3.677841110529857*^9, 3.677841632447709*^9, 3.677841728715215*^9, {
   3.6778419087105103`*^9, 3.677841933417924*^9}, 3.67784199839964*^9, 
   3.6778448263013873`*^9, 3.677847694392432*^9, 3.677847737499898*^9, 
   3.6778490120037956`*^9, 3.677849047989854*^9, 3.677849131298619*^9, {
   3.677849187423829*^9, 3.677849208057009*^9}, 3.6778496105010276`*^9, 
   3.6778499444711294`*^9, 3.6778499978241816`*^9, 3.677850130138749*^9, 
   3.677850519420015*^9, {3.677850622356902*^9, 3.677850636950737*^9}, {
   3.6778507155592337`*^9, 3.677850733249245*^9}, {3.6778509480625315`*^9, 
   3.677851004884782*^9}, 3.677851336887771*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"QLH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.6778391135066338`*^9, {3.677839144077382*^9, 3.6778392771469936`*^9}, {
   3.6778393133960667`*^9, 3.677839365343038*^9}, 3.6778416248142724`*^9, {
   3.6778453832442427`*^9, 3.677845383344248*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1"},
     {"0", 
      RowBox[{"-", "1"}], "0"},
     {"1", "0", "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394427994685`*^9, 3.6778394737812405`*^9, 3.677839854482015*^9, 
   3.6778401283076773`*^9, 3.6778402881428194`*^9, 3.677840321949753*^9, 
   3.677840604591919*^9, 3.6778408352241106`*^9, 3.677840956938072*^9, 
   3.677841110544858*^9, 3.67784163246171*^9, 3.677841728728216*^9, {
   3.6778419087255116`*^9, 3.6778419334349246`*^9}, 3.6778419984146414`*^9, 
   3.677844826314388*^9, 3.677847694410434*^9, 3.6778477375178995`*^9, 
   3.6778490120147963`*^9, 3.6778490480028543`*^9, 3.6778491313106194`*^9, {
   3.6778491874338293`*^9, 3.67784920806801*^9}, 3.6778496105120287`*^9, 
   3.67784994448213*^9, 3.677849997836182*^9, 3.67785013015075*^9, 
   3.6778505194310155`*^9, {3.677850622367903*^9, 3.6778506369607377`*^9}, {
   3.677850715569234*^9, 3.6778507332612457`*^9}, {3.6778509480725327`*^9, 
   3.6778510048967824`*^9}, 3.6778513369037724`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PLH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.6778391135066338`*^9, {3.677839144077382*^9, 3.6778392771469936`*^9}, {
   3.6778393133960667`*^9, 3.6778393717014017`*^9}, 3.6778416265453715`*^9, {
   3.677845381242128*^9, 3.6778453813661346`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"160.95`"},
     {"0.`"},
     {"15.`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394428154693`*^9, 3.6778394737952414`*^9, 3.677839854495016*^9, 
   3.677840128322678*^9, 3.677840288172821*^9, 3.677840321961754*^9, 
   3.67784060460492*^9, 3.6778408352381115`*^9, 3.6778409569560733`*^9, 
   3.677841110558859*^9, 3.677841632476711*^9, 3.677841728742217*^9, {
   3.677841908740512*^9, 3.677841933450926*^9}, 3.677841998430642*^9, 
   3.6778448263283887`*^9, 3.677847694429435*^9, 3.6778477375379004`*^9, 
   3.677849012026797*^9, 3.6778490480158553`*^9, 3.67784913132162*^9, {
   3.67784918744483*^9, 3.6778492080810103`*^9}, 3.6778496106710377`*^9, 
   3.6778499444931307`*^9, 3.6778499978481827`*^9, 3.6778501301617503`*^9, 
   3.677850519441016*^9, {3.6778506223789034`*^9, 3.6778506369707384`*^9}, {
   3.677850715579234*^9, 3.6778507332732463`*^9}, {3.6778509480835333`*^9, 
   3.6778510049077835`*^9}, 3.677851336919773*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"TLH1toTorso", "=", " ", 
   RowBox[{"(", GridBox[{
      {"1", "0", "0", "0"},
      {"0", "0", "1", "98"},
      {"0", 
       RowBox[{"-", "1"}], "0", "100"},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TLH6toTorso", "=", 
   RowBox[{"TLH1toTorso", ".", "TLH6to1"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"TLH6toTorso", "//", "MatrixForm"}], "\[IndentingNewLine]"}], "Input",
 CellChangeTimes->{
  3.6778391135066338`*^9, {3.677839144077382*^9, 3.6778392771469936`*^9}, {
   3.6778393133960667`*^9, 3.6778393717014017`*^9}, {3.6778394670238543`*^9, 
   3.6778394691469755`*^9}, {3.677840007226752*^9, 3.677840009483881*^9}, 
   3.6778416295235415`*^9, {3.677845372921652*^9, 3.677845378094948*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0.`", "0.`", "1.`", "160.95`"},
     {"1.`", "0.`", "0.`", "113.`"},
     {"0.`", "1.`", "0.`", "100.`"},
     {"0.`", "0.`", "0.`", "1.`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778394428324704`*^9, 3.677839473808242*^9, 3.6778398545080166`*^9, 
   3.677840128337679*^9, 3.677840288200823*^9, 3.677840321976754*^9, 
   3.677840604617921*^9, 3.6778408352541122`*^9, 3.6778409569740744`*^9, 
   3.67784111057286*^9, 3.6778416324937115`*^9, 3.677841728758218*^9, {
   3.6778419087585135`*^9, 3.677841933469927*^9}, 3.677841998448643*^9, 
   3.677844826342389*^9, 3.6778476944504356`*^9, 3.677847737554901*^9, 
   3.677849012037798*^9, 3.6778490480298557`*^9, 3.6778491313346205`*^9, {
   3.6778491874558306`*^9, 3.6778492080940113`*^9}, 3.677849610685038*^9, 
   3.6778499445051317`*^9, 3.6778499978621836`*^9, 3.6778501301727514`*^9, 
   3.6778505194530168`*^9, {3.6778506223909044`*^9, 3.677850636983739*^9}, {
   3.677850715591235*^9, 3.677850733285247*^9}, {3.6778509480955334`*^9, 
   3.677851004920784*^9}, 3.677851336935774*^9}]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["Right Hand", "Section",
 CellChangeTimes->{{3.6778395573720217`*^9, 3.677839581715414*^9}}],

Cell[BoxData[{
 RowBox[{"DHPRH", ":=", 
  RowBox[{"(", GridBox[{
     {"aa", "b", "c", "\[Theta]"},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      SubscriptBox["\[Theta]RH", "1"]},
     {
      SubscriptBox["lh", "1"], "0", 
      FractionBox[
       RowBox[{"-", "\[Pi]"}], "2"], 
      RowBox[{
       SubscriptBox["\[Theta]RH", "2"], "-", 
       FractionBox["\[Pi]", "2"]}]},
     {"0", 
      SubscriptBox["lh", "2"], 
      FractionBox["\[Pi]", "2"], 
      SubscriptBox["\[Theta]RH", "3"]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]RH", "4"], "+", "\[Pi]"}]},
     {"0", 
      SubscriptBox["lh", "3"], "0", 
      SubscriptBox["\[Theta]RH", "5"]}
    }], ")"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RH", "1"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RH", "2"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RH", "3"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RH", "4"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]RH", "5"], ":=", "0"}], ";"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["lh", "1"], "=", "0.015"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["lh", "2"], "=", "0.105"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["lh", "3"], "=", "0.05595"}], ";"}], "*)"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["lh", "1"], "=", "15"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["lh", "2"], "=", "105"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["lh", "3"], "=", "55.95"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "6"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"q", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{"Cos", "[", 
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Cos", "[", 
            RowBox[{"DHPRH", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"Sin", "[", 
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Sin", "[", 
            RowBox[{"DHPRH", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {"0", 
         RowBox[{"Sin", "[", 
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
         RowBox[{"Cos", "[", 
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "6"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"a", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{
          RowBox[{"DHPRH", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRH", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"DHPRH", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"i", "+", "1"}], ",", "2"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"QRH6to1", "=", 
   RowBox[{
    RowBox[{"q", "[", "1", "]"}], ".", 
    RowBox[{"q", "[", "2", "]"}], ".", 
    RowBox[{"q", "[", "3", "]"}], ".", 
    RowBox[{"q", "[", "4", "]"}], ".", 
    RowBox[{"q", "[", "5", "]"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"PRH6to1", "=", 
   RowBox[{
    RowBox[{"a", "[", "1", "]"}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"a", "[", "2", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"a", "[", "3", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"a", "[", "4", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"a", "[", "5", "]"}]}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"TRH6to1", "=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRH6to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.677839601788562*^9, 3.6778396037856765`*^9}, 
   3.677839655466632*^9, {3.677839709856743*^9, 3.677839907935073*^9}, {
   3.6778408748233757`*^9, 3.677840888505158*^9}, {3.6778410377846966`*^9, 
   3.677841043335014*^9}, {3.6778415946445465`*^9, 3.6778415980467415`*^9}, {
   3.677841803733506*^9, 3.6778418195634117`*^9}, {3.677841973883238*^9, 
   3.6778419819867015`*^9}, {3.6778454350292044`*^9, 3.6778454515581493`*^9}, 
   3.6778490410994596`*^9, {3.6778490990847764`*^9, 3.6778490994897995`*^9}, {
   3.6778495374908514`*^9, 3.6778495985573444`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TRH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677839601788562*^9, 3.6778396037856765`*^9}, 
   3.677839655466632*^9, {3.677839709856743*^9, 3.677839907935073*^9}, 
   3.677841602777012*^9, {3.6778454536152678`*^9, 3.6778454536812716`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1", "160.95`"},
     {"0", 
      RowBox[{"-", "1"}], "0", "0.`"},
     {"1", "0", "0", 
      RowBox[{"-", "15.`"}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778491313826237`*^9, {3.6778491875028334`*^9, 3.6778492081450143`*^9}, 
   3.6778496107370415`*^9, 3.6778499445521345`*^9, 3.677849997911186*^9, 
   3.6778501302197537`*^9, 3.67785051950202*^9, {3.677850622439907*^9, 
   3.677850637033742*^9}, {3.6778507156352377`*^9, 3.67785073333525*^9}, {
   3.6778509481455364`*^9, 3.677851004972787*^9}, 3.677851336985777*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.677849105112121*^9, 3.677849105936168*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"QRH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677839601788562*^9, 3.6778396037856765`*^9}, 
   3.677839655466632*^9, {3.677839709856743*^9, 3.677839882260604*^9}, 
   3.6778416050171404`*^9, {3.6778454552083583`*^9, 3.6778454552593613`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1"},
     {"0", 
      RowBox[{"-", "1"}], "0"},
     {"1", "0", "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677840128397682*^9, 3.6778402882858276`*^9, 3.6778403220437584`*^9, 
   3.6778406046859245`*^9, 3.677840835332117*^9, 3.6778409570460787`*^9, 
   3.6778411106598644`*^9, 3.677841632556715*^9, 3.6778417288292217`*^9, {
   3.677841908831517*^9, 3.6778419335669317`*^9}, 3.6778419985246477`*^9, 
   3.677844826408393*^9, 3.67784769452344*^9, 3.6778477376289053`*^9, 
   3.677849012096801*^9, 3.67784904809186*^9, 3.6778491314016247`*^9, {
   3.6778491875218344`*^9, 3.6778492081660156`*^9}, 3.6778496107570424`*^9, 
   3.677849944574136*^9, 3.677849997930187*^9, 3.6778501302417555`*^9, 
   3.677850519521021*^9, {3.677850622458908*^9, 3.677850637052743*^9}, {
   3.677850715655239*^9, 3.6778507333562512`*^9}, {3.677850948165538*^9, 
   3.6778510049927883`*^9}, 3.6778513370107784`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PRH6to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677839601788562*^9, 3.6778396037856765`*^9}, 
   3.677839655466632*^9, {3.677839709856743*^9, 3.6778399140504227`*^9}, 
   3.6778416067002363`*^9, {3.6778454569224567`*^9, 3.67784545698046*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"160.95`"},
     {"0.`"},
     {
      RowBox[{"-", "15.`"}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778401284126835`*^9, 3.677840288313829*^9, 3.6778403220577593`*^9, 
   3.6778406046999254`*^9, 3.677840835348118*^9, 3.677840957060079*^9, 
   3.677841110694866*^9, 3.677841632568716*^9, 3.6778417288432226`*^9, {
   3.677841908845518*^9, 3.677841933582933*^9}, 3.6778419985386486`*^9, 
   3.677844826422394*^9, 3.6778476945424414`*^9, 3.6778477376459064`*^9, 
   3.6778490121098013`*^9, 3.67784904810686*^9, 3.6778491314136257`*^9, {
   3.677849187532835*^9, 3.6778492081800165`*^9}, 3.677849610770043*^9, 
   3.6778499445881367`*^9, 3.6778499979421883`*^9, 3.677850130254756*^9, 
   3.6778505195320215`*^9, {3.6778506224719095`*^9, 3.6778506370657434`*^9}, {
   3.6778507156662397`*^9, 3.677850733368252*^9}, {3.6778509481785383`*^9, 
   3.677851005005789*^9}, 3.677851337027779*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"TRH1toTorso", ":=", 
   RowBox[{"(", GridBox[{
      {"1", "0", "0", "0"},
      {"0", "0", "1", 
       RowBox[{"-", "98"}]},
      {"0", 
       RowBox[{"-", "1"}], "0", "100"},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TRH6toTorso", ":=", 
   RowBox[{"TRH1toTorso", ".", "TRH6to1"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"TRH6toTorso", "//", "MatrixForm"}]}], "Input",
 CellChangeTimes->{{3.6778399162935505`*^9, 3.677840049192152*^9}, 
   3.6778416097274094`*^9, {3.6778454597286167`*^9, 3.677845464622897*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0.`", "0.`", "1.`", "160.95`"},
     {"1.`", "0.`", "0.`", 
      RowBox[{"-", "113.`"}]},
     {"0.`", "1.`", "0.`", "100.`"},
     {"0.`", "0.`", "0.`", "1.`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677840128425684*^9, 3.6778402883448305`*^9, 3.67784032206976*^9, 
   3.6778406047139263`*^9, 3.6778408353651185`*^9, 3.67784095707408*^9, 
   3.677841110734869*^9, 3.677841632584717*^9, 3.6778417288612237`*^9, {
   3.677841908861519*^9, 3.677841933597934*^9}, 3.677841998553649*^9, 
   3.677844826437395*^9, 3.677847694559442*^9, 3.677847737668908*^9, 
   3.677849012123802*^9, 3.6778490481238613`*^9, 3.677849131425626*^9, {
   3.677849187545836*^9, 3.677849208194017*^9}, 3.677849610783044*^9, 
   3.677849944603137*^9, 3.677849997956189*^9, 3.6778501302677565`*^9, 
   3.677850519543022*^9, {3.67785062248491*^9, 3.6778506370807447`*^9}, {
   3.67785071567924*^9, 3.677850733381253*^9}, {3.6778509481905394`*^9, 
   3.6778510050207896`*^9}, 3.6778513370447807`*^9}]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["Left Leg", "Section",
 CellChangeTimes->{{3.677840251231708*^9, 3.6778402689997244`*^9}, {
  3.6778403680963926`*^9, 3.6778403692254567`*^9}}],

Cell[BoxData[
 RowBox[{"DHPLL", ":=", 
  RowBox[{"(", GridBox[{
     {"aa", "b", "c", "\[Theta]"},
     {"0", "0", 
      FractionBox["\[Pi]", "4"], 
      SubscriptBox["\[Theta]LL", "1"]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LL", "2"], "+", 
       FractionBox["\[Pi]", "2"]}]},
     {
      SubscriptBox["ll", "1"], "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LL", "3"], "+", "\[Pi]"}]},
     {
      SubscriptBox["ll", "2"], "0", "0", 
      SubscriptBox["\[Theta]LL", "4"]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]LL", "5"], "+", "\[Pi]"}]},
     {
      SubscriptBox["ll", "3"], "0", "0", 
      RowBox[{
       SubscriptBox["\[Theta]LL", "6"], "+", "\[Pi]"}]}
    }], ")"}]}]], "Input",
 CellChangeTimes->{{3.6778403711915693`*^9, 3.6778403993231783`*^9}, {
  3.67784047103328*^9, 3.6778404864271603`*^9}, {3.677840895339549*^9, 
  3.67784089903376*^9}, {3.6778410525435405`*^9, 3.6778410549066753`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LL", "1"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LL", "2"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LL", "3"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LL", "4"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]LL", "5"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]LL", "6"], ":=", "0"}], ";"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]LL", "1"], ":=", "0"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["\[Theta]LL", "2"], ":=", "0"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["\[Theta]LL", "3"], ":=", 
     FractionBox["\[Pi]", "4"]}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["\[Theta]LL", "4"], ":=", 
     FractionBox["\[Pi]", "2"]}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["\[Theta]LL", "5"], ":=", "0"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["\[Theta]LL", "6"], ":=", "0"}], ";"}], "*)"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["ll", "1"], "=", "100"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["ll", "2"], "=", "102.9"}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["ll", "3"], "=", "45.19"}], ";"}], "*)"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "7"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"q", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{"Cos", "[", 
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Cos", "[", 
            RowBox[{"DHPLL", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"Sin", "[", 
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Sin", "[", 
            RowBox[{"DHPLL", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {"0", 
         RowBox[{"Sin", "[", 
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
         RowBox[{"Cos", "[", 
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "7"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"a", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{
          RowBox[{"DHPLL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPLL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"DHPLL", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"i", "+", "1"}], ",", "2"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"QLL7to1", "=", 
   RowBox[{
    RowBox[{"q", "[", "1", "]"}], ".", 
    RowBox[{"q", "[", "2", "]"}], ".", 
    RowBox[{"q", "[", "3", "]"}], ".", 
    RowBox[{"q", "[", "4", "]"}], ".", 
    RowBox[{"q", "[", "5", "]"}], ".", 
    RowBox[{"q", "[", "6", "]"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"PLL7to1", "=", 
   RowBox[{
    RowBox[{"a", "[", "1", "]"}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"a", "[", "2", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"a", "[", "3", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"a", "[", "4", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"a", "[", "5", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"q", "[", "5", "]"}], ".", 
     RowBox[{"a", "[", "6", "]"}]}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"TLL7to1", "=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PLL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.677840422219488*^9, 3.677840436396299*^9}, {
   3.6778404898793583`*^9, 3.6778405869909124`*^9}, {3.677840624920082*^9, 
   3.6778406477493877`*^9}, {3.6778409070402184`*^9, 3.677840947388526*^9}, {
   3.6778410568627872`*^9, 3.6778410593839316`*^9}, {3.677841569597114*^9, 
   3.6778415717502375`*^9}, {3.6778417557347608`*^9, 3.677841769471546*^9}, 
   3.6778419634896436`*^9, {3.677844809824445*^9, 3.6778448238112445`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TLL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677840422219488*^9, 3.677840436396299*^9}, {
   3.6778404898793583`*^9, 3.6778405869909124`*^9}, {3.677840624920082*^9, 
   3.6778406507585597`*^9}, 3.6778415729433055`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1", "0"},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      FractionBox["1", 
       SqrtBox["2"]], "0", 
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         SubscriptBox["ll", "1"], 
         SqrtBox["2"]]}], "-", 
       FractionBox[
        SubscriptBox["ll", "2"], 
        SqrtBox["2"]], "-", 
       FractionBox[
        SubscriptBox["ll", "3"], 
        SqrtBox["2"]]}]},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], "0", 
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         SubscriptBox["ll", "1"], 
         SqrtBox["2"]]}], "-", 
       FractionBox[
        SubscriptBox["ll", "2"], 
        SqrtBox["2"]], "-", 
       FractionBox[
        SubscriptBox["ll", "3"], 
        SqrtBox["2"]]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778408354311223`*^9, 3.677840957168085*^9, 3.677841110822874*^9, 
   3.67784163264472*^9, 3.6778417292322445`*^9, {3.677841908928523*^9, 
   3.6778419336919394`*^9}, 3.677841998622653*^9, 3.6778448264953985`*^9, 
   3.677847694631446*^9, 3.6778477377359114`*^9, 3.6778490121808057`*^9, 
   3.6778490481858654`*^9, 3.6778491314846296`*^9, {3.6778491876068397`*^9, 
   3.67784920825502*^9}, 3.677849610839047*^9, 3.6778499446591406`*^9, 
   3.677849998014192*^9, 3.67785013032076*^9, 3.6778505195980253`*^9, {
   3.6778506225429134`*^9, 3.677850637137748*^9}, {3.677850715733243*^9, 
   3.677850733440256*^9}, {3.6778509482455425`*^9, 3.6778510050817933`*^9}, 
   3.677851337109784*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"QLL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677840422219488*^9, 3.677840436396299*^9}, {
   3.6778404898793583`*^9, 3.6778405869909124`*^9}, {3.677840624920082*^9, 
   3.677840652706671*^9}, 3.6778415749314194`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1"},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      FractionBox["1", 
       SqrtBox["2"]], "0"},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677840835446123*^9, 3.6778409571830864`*^9, 3.677841110867876*^9, 
   3.6778416326577215`*^9, 3.677841729245246*^9, {3.677841908942524*^9, 
   3.6778419337099404`*^9}, 3.6778419986376543`*^9, 3.6778448265183997`*^9, 
   3.677847694648447*^9, 3.677847737753913*^9, 3.6778490121918063`*^9, 
   3.6778490482008657`*^9, 3.67784913149663*^9, {3.67784918761884*^9, 
   3.677849208268021*^9}, 3.6778496109030504`*^9, 3.677849944671141*^9, 
   3.6778499980251927`*^9, 3.6778501303327603`*^9, 3.677850519609026*^9, {
   3.677850622555914*^9, 3.677850637149749*^9}, {3.677850715744244*^9, 
   3.677850733452257*^9}, {3.6778509482575426`*^9, 3.677851005094794*^9}, 
   3.677851337128785*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PLL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.677840422219488*^9, 3.677840436396299*^9}, {
   3.6778404898793583`*^9, 3.6778405869909124`*^9}, {3.677840624920082*^9, 
   3.677840655939856*^9}, 3.6778415763555007`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0"},
     {
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         SubscriptBox["ll", "1"], 
         SqrtBox["2"]]}], "-", 
       FractionBox[
        SubscriptBox["ll", "2"], 
        SqrtBox["2"]], "-", 
       FractionBox[
        SubscriptBox["ll", "3"], 
        SqrtBox["2"]]}]},
     {
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         SubscriptBox["ll", "1"], 
         SqrtBox["2"]]}], "-", 
       FractionBox[
        SubscriptBox["ll", "2"], 
        SqrtBox["2"]], "-", 
       FractionBox[
        SubscriptBox["ll", "3"], 
        SqrtBox["2"]]}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677840835462124*^9, 3.6778409571950865`*^9, 3.6778411109028783`*^9, 
   3.677841632671722*^9, 3.6778417292592463`*^9, {3.6778419089555244`*^9, 
   3.677841933726941*^9}, 3.677841998653655*^9, 3.677844826539401*^9, 
   3.677847694665448*^9, 3.6778477377699137`*^9, 3.6778490122028065`*^9, 
   3.6778490482148666`*^9, 3.677849131507631*^9, {3.677849187629841*^9, 
   3.677849208281022*^9}, 3.6778496109160514`*^9, 3.677849944684142*^9, 
   3.6778499980351934`*^9, 3.6778501303437605`*^9, 3.6778505196200266`*^9, {
   3.6778506225689144`*^9, 3.6778506371627493`*^9}, {3.677850715757245*^9, 
   3.6778507334652576`*^9}, {3.6778509482695436`*^9, 3.677851005107795*^9}, 
   3.6778513371467857`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"TLL1toTorso", ":=", 
   RowBox[{"(", GridBox[{
      {"1", "0", "0", "0"},
      {"0", 
       FractionBox[
        SqrtBox["2"], "2"], 
       RowBox[{"-", 
        FractionBox[
         SqrtBox["2"], "2"]}], "50"},
      {"0", 
       FractionBox[
        SqrtBox["2"], "2"], 
       FractionBox[
        SqrtBox["2"], "2"], 
       RowBox[{"-", "85"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TLL7toTorso", "=", 
   RowBox[{"TLL1toTorso", ".", "TLL7to1"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"TLL7toTorso", "//", "MatrixForm"}]}], "Input",
 CellChangeTimes->{{3.677840422219488*^9, 3.677840436396299*^9}, {
   3.6778404898793583`*^9, 3.6778405869909124`*^9}, {3.677840624920082*^9, 
   3.677840821020298*^9}, 3.6778415798266993`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", "1", "0"},
     {"0", "1", "0", "50"},
     {
      RowBox[{"-", "1"}], "0", "0", 
      RowBox[{
       RowBox[{"-", "85"}], "+", 
       RowBox[{
        SqrtBox["2"], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", 
           FractionBox[
            SubscriptBox["ll", "1"], 
            SqrtBox["2"]]}], "-", 
          FractionBox[
           SubscriptBox["ll", "2"], 
           SqrtBox["2"]], "-", 
          FractionBox[
           SubscriptBox["ll", "3"], 
           SqrtBox["2"]]}], ")"}]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677840835478125*^9, 3.6778409572120876`*^9, 3.6778411109418807`*^9, 
   3.6778416326867228`*^9, 3.6778417292812476`*^9, {3.6778419089705253`*^9, 
   3.6778419337449427`*^9}, 3.6778419986716557`*^9, 3.6778448265614023`*^9, 
   3.677847694685449*^9, 3.677847737789915*^9, 3.677849012218808*^9, 
   3.6778490482328677`*^9, 3.6778491315216317`*^9, {3.6778491876428413`*^9, 
   3.677849208295023*^9}, 3.6778496109300523`*^9, 3.6778499446971426`*^9, 
   3.677849998047194*^9, 3.677850130356762*^9, 3.6778505196340275`*^9, {
   3.6778506225839157`*^9, 3.67785063717675*^9}, {3.6778507157702456`*^9, 
   3.6778507334802585`*^9}, {3.6778509482845445`*^9, 3.6778510051217957`*^9}, 
   3.677851337166787*^9}]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["Right Leg", "Section",
 CellChangeTimes->{{3.6778411362523284`*^9, 3.6778411683511643`*^9}}],

Cell[BoxData[{
 RowBox[{"DHPRL", ":=", 
  RowBox[{"(", GridBox[{
     {"aa", "b", "c", "\[Theta]"},
     {"0", "0", 
      FractionBox["\[Pi]", "4"], 
      SubscriptBox["\[Theta]RL", "1"]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      RowBox[{
       SubscriptBox["\[Theta]RL", "2"], "-", 
       FractionBox["\[Pi]", "2"]}]},
     {
      SubscriptBox["ll", "1"], "0", 
      RowBox[{"-", 
       FractionBox["\[Pi]", "2"]}], 
      SubscriptBox["\[Theta]RL", "3"]},
     {
      SubscriptBox["ll", "2"], "0", "0", 
      SubscriptBox["\[Theta]RL", "4"]},
     {"0", "0", 
      FractionBox["\[Pi]", "2"], 
      SubscriptBox["\[Theta]RL", "5"]},
     {
      SubscriptBox["ll", "3"], "0", "0", 
      SubscriptBox["\[Theta]RL", "6"]}
    }], ")"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RL", "1"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RL", "2"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RL", "3"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RL", "4"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]RL", "5"], ":=", "0"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]RL", "6"], ":=", "0"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["ll", "1"], "=", "100"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["ll", "2"], "=", "102.9"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ll", "3"], "=", "45.19"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "7"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"q", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{"Cos", "[", 
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Cos", "[", 
            RowBox[{"DHPRL", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"Sin", "[", 
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}], 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}], 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"Sin", "[", 
            RowBox[{"DHPRL", "[", 
             RowBox[{"[", 
              RowBox[{
               RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {"0", 
         RowBox[{"Sin", "[", 
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}], 
         RowBox[{"Cos", "[", 
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "3"}], "]"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "<", "7"}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     RowBox[{"a", "[", "i_", "]"}], ":=", 
     RowBox[{"(", GridBox[{
        {
         RowBox[{
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Cos", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{
          RowBox[{"DHPRL", "[", 
           RowBox[{"[", 
            RowBox[{
             RowBox[{"i", "+", "1"}], ",", "1"}], "]"}], "]"}], 
          RowBox[{"Sin", "[", 
           RowBox[{"DHPRL", "[", 
            RowBox[{"[", 
             RowBox[{
              RowBox[{"i", "+", "1"}], ",", "4"}], "]"}], "]"}], "]"}]}]},
        {
         RowBox[{"DHPRL", "[", 
          RowBox[{"[", 
           RowBox[{
            RowBox[{"i", "+", "1"}], ",", "2"}], "]"}], "]"}]}
       }], ")"}]}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"QRL7to1", "=", 
   RowBox[{
    RowBox[{"q", "[", "1", "]"}], ".", 
    RowBox[{"q", "[", "2", "]"}], ".", 
    RowBox[{"q", "[", "3", "]"}], ".", 
    RowBox[{"q", "[", "4", "]"}], ".", 
    RowBox[{"q", "[", "5", "]"}], ".", 
    RowBox[{"q", "[", "6", "]"}]}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"PRL7to1", "=", 
   RowBox[{
    RowBox[{"a", "[", "1", "]"}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"a", "[", "2", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"a", "[", "3", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"a", "[", "4", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"a", "[", "5", "]"}]}], "+", 
    RowBox[{
     RowBox[{"q", "[", "1", "]"}], ".", 
     RowBox[{"q", "[", "2", "]"}], ".", 
     RowBox[{"q", "[", "3", "]"}], ".", 
     RowBox[{"q", "[", "4", "]"}], ".", 
     RowBox[{"q", "[", "5", "]"}], ".", 
     RowBox[{"a", "[", "6", "]"}]}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"TRL7to1", "=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"2", ",", "1"}], "]"}], "]"}]},
      {
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "2"}], "]"}], "]"}], 
       RowBox[{"QRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "3"}], "]"}], "]"}], 
       RowBox[{"PRL7to1", "[", 
        RowBox[{"[", 
         RowBox[{"3", ",", "1"}], "]"}], "]"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.6778412072943916`*^9, 3.6778413921079626`*^9}, {
   3.677841491090624*^9, 3.6778415388803573`*^9}, {3.6778416904490266`*^9, 
   3.6778417189676576`*^9}, 3.677841956210227*^9, 3.677850443729686*^9, {
   3.6778507047956176`*^9, 3.67785070850683*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TRL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.6778412072943916`*^9, 3.6778413965002136`*^9}, 
   3.6778414887204885`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", 
      RowBox[{"-", "1"}], "0.`"},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], "0", 
      RowBox[{"-", "175.42612134457056`"}]},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      FractionBox["1", 
       SqrtBox["2"]], "0", 
      RowBox[{"-", "175.42612134457056`"}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677841633040743*^9, 3.677841729344251*^9, {3.677841909032529*^9, 
   3.677841933828947*^9}, 3.6778419987346597`*^9, 3.677844826652407*^9, 
   3.6778476947434525`*^9, 3.677847737846918*^9, 3.6778490122668104`*^9, 
   3.677849048288871*^9, 3.6778491315706344`*^9, {3.6778491876938443`*^9, 
   3.6778492083450255`*^9}, 3.6778496109840555`*^9, 3.6778499448181496`*^9, 
   3.677849998096197*^9, 3.6778501304107647`*^9, 3.67785051968603*^9, {
   3.6778506226339183`*^9, 3.6778506372277527`*^9}, {3.6778507158122478`*^9, 
   3.6778507335272613`*^9}, {3.6778509483305473`*^9, 3.677851005171798*^9}, 
   3.6778513372227907`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"QRL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.6778412072943916`*^9, 3.677841398906351*^9}, {
  3.6778415039743605`*^9, 3.6778415208993287`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", "0", 
      RowBox[{"-", "1"}]},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], "0"},
     {
      RowBox[{"-", 
       FractionBox["1", 
        SqrtBox["2"]]}], 
      FractionBox["1", 
       SqrtBox["2"]], "0"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677841633056744*^9, 3.677841729358252*^9, {3.6778419090475297`*^9, 
   3.677841933842948*^9}, 3.677841998755661*^9, 3.677844826669408*^9, 
   3.6778476947604537`*^9, 3.677847737863919*^9, 3.6778490122788115`*^9, 
   3.677849048303872*^9, 3.677849131582635*^9, {3.6778491877068453`*^9, 
   3.6778492083580265`*^9}, 3.677849610998056*^9, 3.67784994483015*^9, 
   3.6778499981081977`*^9, 3.6778501304227657`*^9, 3.677850519704031*^9, {
   3.677850622650919*^9, 3.6778506372457542`*^9}, {3.677850715828249*^9, 
   3.6778507335402617`*^9}, {3.677850948346548*^9, 3.677851005188799*^9}, 
   3.677851337246792*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PRL7to1", "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.6778412072943916`*^9, 3.677841398906351*^9}, 
   3.677841523141457*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0.`"},
     {
      RowBox[{"-", "175.42612134457056`"}]},
     {
      RowBox[{"-", "175.42612134457056`"}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.677841633069745*^9, 3.677841729376253*^9, {3.677841909064531*^9, 
   3.6778419338609486`*^9}, 3.6778419987736616`*^9, 3.6778448267094107`*^9, 
   3.6778476947794547`*^9, 3.6778477378839197`*^9, 3.677849012290812*^9, 
   3.677849048320873*^9, 3.677849131594636*^9, {3.677849187719846*^9, 
   3.677849208371027*^9}, 3.677849611012057*^9, 3.677849944843151*^9, 
   3.6778499981201982`*^9, 3.677850130435766*^9, 3.6778505197190323`*^9, {
   3.67785062266492*^9, 3.677850637261755*^9}, {3.67785071584225*^9, 
   3.6778507335542626`*^9}, {3.677850948360549*^9, 3.6778510052068005`*^9}, 
   3.677851337270793*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"TRL1toTorso", ":=", 
   RowBox[{"(", GridBox[{
      {
       RowBox[{"-", "1"}], "0", "0", "0"},
      {"0", 
       RowBox[{"-", 
        FractionBox[
         SqrtBox["2"], "2"]}], 
       FractionBox[
        SqrtBox["2"], "2"], 
       RowBox[{"-", "50"}]},
      {"0", 
       FractionBox[
        SqrtBox["2"], "2"], 
       FractionBox[
        SqrtBox["2"], "2"], 
       RowBox[{"-", "85"}]},
      {"0", "0", "0", "1"}
     }], ")"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TRL7toTorso", ":=", 
   RowBox[{"TRL1toTorso", ".", "TRL7to1"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"TRL7toTorso", "//", "MatrixForm"}]}], "Input",
 CellChangeTimes->{{3.6778414054097233`*^9, 3.6778414304861574`*^9}, {
  3.677841477277834*^9, 3.6778414809740453`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0.`", "0.`", "1.`", "0.`"},
     {"0.`", "0.9999999999999998`", "0.`", 
      RowBox[{"-", "50.`"}]},
     {
      RowBox[{"-", "0.9999999999999998`"}], "0.`", "0.`", 
      RowBox[{"-", "333.0899999999999`"}]},
     {"0.`", "0.`", "0.`", "1.`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{
  3.6778416330847454`*^9, 3.6778417293962545`*^9, {3.6778419090815315`*^9, 
   3.67784193387995*^9}, 3.677841998795663*^9, 3.677844826740412*^9, 
   3.677847694800456*^9, 3.6778477379019213`*^9, 3.677849012304813*^9, 
   3.6778490483378735`*^9, 3.6778491316076365`*^9, {3.677849187734847*^9, 
   3.677849208385028*^9}, 3.677849611028058*^9, 3.6778499448551517`*^9, 
   3.6778499981331987`*^9, 3.677850130451767*^9, 3.677850519737033*^9, {
   3.677850622680921*^9, 3.677850637279756*^9}, {3.67785071585725*^9, 
   3.6778507335672636`*^9}, {3.67785094837455*^9, 3.6778510052218013`*^9}, 
   3.6778513372957945`*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.6778465196422405`*^9, 3.6778465203562813`*^9}, 
   3.6778465832318773`*^9},
 NumberMarks->False]
}, Open  ]],

Cell[CellGroupData[{

Cell["Plot", "Section",
 CellChangeTimes->{{3.677846588657188*^9, 3.6778466171668186`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"head", " ", "=", " ", 
   RowBox[{"Graphics3D", "[", 
    RowBox[{"Line", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"THD3toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"THD3toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"THD3toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}]}], "}"}], "]"}], 
    "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"righthand", "=", " ", 
   RowBox[{"Graphics3D", "[", 
    RowBox[{"Line", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "100"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TRH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TRH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}]}], "}"}], "]"}], 
    "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"lefthand", "=", 
   RowBox[{"Graphics3D", "[", 
    RowBox[{"Line", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "100"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TLH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TLH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TLH1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TLH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TLH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TLH6toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}]}], "}"}], "]"}], 
    "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"rightleg", " ", "=", " ", 
   RowBox[{"Graphics3D", "[", 
    RowBox[{"Line", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TRL1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRL1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRL1toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"TRL7toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRL7toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
         RowBox[{"TRL7toTorso", "[", 
          RowBox[{"[", 
           RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}]}], "}"}], "]"}], 
    "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"leftleg", " ", "=", " ", 
    RowBox[{"Graphics3D", "[", 
     RowBox[{"Line", "[", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"TLL1toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
          RowBox[{"TLL1toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
          RowBox[{"TLL1toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"TLL7toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", 
          RowBox[{"TLL7toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", 
          RowBox[{"TLL7toTorso", "[", 
           RowBox[{"[", 
            RowBox[{"3", ",", "4"}], "]"}], "]"}]}], "}"}]}], "}"}], "]"}], 
     "]"}]}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{"Show", "[", 
  RowBox[{
  "head", ",", "righthand", ",", "lefthand", ",", "rightleg", ",", "leftleg", 
   ",", 
   RowBox[{"Axes", "\[Rule]", "True"}]}], "]"}]}], "Input",
 CellChangeTimes->{{3.6778466504147205`*^9, 3.67784665512599*^9}, {
   3.677846702246685*^9, 3.6778467320753913`*^9}, {3.677846929777699*^9, 
   3.677846933007884*^9}, {3.6778471960299277`*^9, 3.6778472520041294`*^9}, {
   3.677847371010936*^9, 3.677847378615371*^9}, 3.677847432550456*^9, {
   3.6778474848564477`*^9, 3.6778476222483063`*^9}, {3.6778477135505285`*^9, 
   3.677847752162737*^9}, {3.6778478695454507`*^9, 3.677847922760494*^9}, {
   3.677848672584382*^9, 3.6778487841007605`*^9}, {3.6778488637593164`*^9, 
   3.677848948214147*^9}, {3.6778491141906404`*^9, 3.6778491178298483`*^9}, {
   3.677849171056893*^9, 3.6778491790603504`*^9}, {3.677849735164158*^9, 
   3.677849937099708*^9}, {3.6778500367934103`*^9, 3.6778500572915826`*^9}, {
   3.6778501182520695`*^9, 3.677850126859562*^9}, {3.6778501583383617`*^9, 
   3.677850322981779*^9}, {3.67785035883683*^9, 3.6778504029923553`*^9}, {
   3.6778504608376637`*^9, 3.6778505133726687`*^9}, {3.677850580497508*^9, 
   3.677850616181549*^9}, {3.6778506648933353`*^9, 3.677850666758442*^9}, {
   3.6778507237177*^9, 3.6778507295090313`*^9}, {3.677850791713589*^9, 
   3.6778509643494635`*^9}, {3.6778509974553566`*^9, 
   3.6778510011905704`*^9}, {3.677851095989993*^9, 3.6778511346312027`*^9}, {
   3.677851211220584*^9, 3.677851234485914*^9}, 3.6778512682348447`*^9, {
   3.677851314595496*^9, 3.677851329381342*^9}}],

Cell[BoxData[
 Graphics3DBox[{Line3DBox[{{0, 0, 0}, {0., 0., 126.5}}], 
   Line3DBox[{{0, 0, 0}, {0, 0, 100}, {0, -98, 100}, {160.95, -113., 100.}}], 
   Line3DBox[{{0, 0, 0}, {0, 0, 100}, {0, 98, 100}, {160.95, 113., 100.}}], 
   Line3DBox[{{0, 0, 0}, {0, -50, -85}, {0., -50., -333.0899999999999}}], 
   Line3DBox[{{0, 0, 0}, {0, 50, -85}, {0, 50, -333.09000000000003`}}]},
  Axes->True]], "Output",
 CellChangeTimes->{3.6778513373697987`*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]]
},
WindowSize->{1920, 974},
WindowMargins->{{-9, Automatic}, {Automatic, -9}},
Magnification:>1.6 Inherited,
FrontEndVersion->"10.0 for Microsoft Windows (64-bit) (July 1, 2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[580, 22, 161, 2, 161, "Title"],
Cell[CellGroupData[{
Cell[766, 28, 90, 1, 112, "Section"],
Cell[859, 31, 6477, 193, 564, "Input"],
Cell[CellGroupData[{
Cell[7361, 228, 413, 6, 47, "Input"],
Cell[7777, 236, 1568, 33, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[9382, 274, 413, 6, 47, "Input"],
Cell[9798, 282, 1526, 32, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[11361, 319, 415, 6, 47, "Input"],
Cell[11779, 327, 1474, 31, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[13290, 363, 795, 18, 185, "Input"],
Cell[14088, 383, 1567, 32, 70, "Output"]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[15704, 421, 93, 1, 87, "Section"],
Cell[15800, 424, 8548, 258, 981, "Input"],
Cell[CellGroupData[{
Cell[24373, 686, 290, 5, 47, "Input"],
Cell[24666, 693, 1562, 33, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[26265, 731, 288, 5, 47, "Input"],
Cell[26556, 738, 1522, 32, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[28115, 775, 290, 5, 47, "Input"],
Cell[28408, 782, 1476, 31, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[29921, 818, 793, 18, 214, "Input"],
Cell[30717, 838, 1579, 32, 70, "Output"]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[32345, 876, 97, 1, 87, "Section"],
Cell[32445, 879, 8701, 258, 1010, "Input"],
Cell[CellGroupData[{
Cell[41171, 1141, 281, 4, 47, "Input"],
Cell[41455, 1147, 1105, 27, 70, "Output"]
}, Open  ]],
Cell[42575, 1177, 92, 1, 47, InheritFromParent],
Cell[CellGroupData[{
Cell[42692, 1182, 283, 4, 47, "Input"],
Cell[42978, 1188, 1448, 31, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[44463, 1224, 282, 4, 47, "Input"],
Cell[44748, 1230, 1430, 31, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[46215, 1266, 611, 16, 185, "Input"],
Cell[46829, 1284, 1514, 32, 70, "Output"]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[48392, 1322, 148, 2, 87, "Section"],
Cell[48543, 1326, 1065, 31, 262, "Input"],
Cell[49611, 1359, 8620, 250, 938, "Input"],
Cell[CellGroupData[{
Cell[58256, 1613, 261, 4, 48, "Input"],
Cell[58520, 1619, 2173, 63, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[60730, 1687, 259, 4, 48, "Input"],
Cell[60992, 1693, 1562, 40, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[62591, 1738, 259, 4, 48, "Input"],
Cell[62853, 1744, 1868, 51, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[64758, 1800, 837, 25, 250, "Input"],
Cell[65598, 1827, 1845, 47, 70, "Output"]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[67492, 1880, 98, 1, 87, "Section"],
Cell[67593, 1883, 8537, 259, 977, "Input"],
Cell[CellGroupData[{
Cell[76155, 2146, 163, 3, 47, "Input"],
Cell[76321, 2151, 1647, 43, 170, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[78005, 2199, 186, 3, 47, "Input"],
Cell[78194, 2204, 1511, 40, 145, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[79742, 2249, 159, 3, 47, "Input"],
Cell[79904, 2254, 1300, 30, 113, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[81241, 2289, 811, 26, 234, "Input"],
Cell[82055, 2317, 1439, 32, 138, "Output"]
}, Open  ]],
Cell[83509, 2352, 145, 3, 47, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[83691, 2360, 91, 1, 112, "Section"],
Cell[CellGroupData[{
Cell[83807, 2365, 6747, 173, 454, "Input"],
Cell[90557, 2540, 445, 7, 748, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)