(************** Content-type: application/mathematica ************** CreatedBy='Mathematica 5.1' Mathematica-Compatible Notebook This notebook can be used with any Mathematica-compatible application, such as Mathematica, MathReader or Publicon. The data for the notebook starts with the line containing stars above. To get the notebook into a Mathematica-compatible application, do one of the following: * Save the data starting with the line of stars above into a file with a name ending in .nb, then open the file inside the application; * Copy the data starting with the line of stars above to the clipboard, then use the Paste menu command inside the application. Data for notebooks contains only printable 7-bit ASCII and can be sent directly in email or through ftp in text mode. Newlines can be CR, LF or CRLF (Unix, Macintosh or MS-DOS style). NOTE: If you modify the data for this notebook not in a Mathematica- compatible application, you must delete the line below containing the word CacheID, otherwise Mathematica-compatible applications may try to use invalid cache data. For more information on notebooks and Mathematica-compatible applications, contact Wolfram Research: web: http://www.wolfram.com email: info@wolfram.com phone: +1-217-398-0700 (U.S.) Notebook reader applications are available free of charge from Wolfram Research. *******************************************************************) (*CacheID: 232*) (*NotebookFileLineBreakTest NotebookFileLineBreakTest*) (*NotebookOptionsPosition[ 137519, 2950]*) (*NotebookOutlinePosition[ 138163, 2972]*) (* CellTagsIndexPosition[ 138119, 2968]*) (*WindowFrame->Normal*) Notebook[{ Cell[CellGroupData[{ Cell["\<\ Mathematical Model for 3D Point-footed, Midleg-Mass, Hipped Walker with Yaw\ \>", "Title"], Cell["\<\ Modeling by Robert D Gregg rdgregg@eecs.berkeley.edu Center for Hybrid and Embedded Software Systems Electrical Engineering and Computer Sciences University of California, Berkeley\ \>", "Subsubtitle"], Cell[BoxData[{ \(<< \ H:\\Screws.m\), "\[IndentingNewLine]", \(<< \ H:\\RobotLinks.m\)}], "Input"], Cell[CellGroupData[{ Cell["Variables", "Section"], Cell[BoxData[{ \(\(x4\ --\) > \ stance\ yaw\ angle\), "\[IndentingNewLine]", \(\(x5\ --\) > \ stance\ roll\ angle\), "\n", \(\(x6\ --\) > \ stance\ pitch\ angle\), "\n", \(\(x7\ --\) > \ swing\ pitch\ angle\)}], "Input"], Cell[CellGroupData[{ Cell["Constants", "Subsection"], Cell[BoxData[{ \(\(w\ --\) > \ hip\ width\), "\n", \(\(l\ --\) > \ leg\ length\), "\n", \(\(Mp\ --\) > \ pelvis/hip\ mass\), "\n", \(\(M\ --\) > \ midleg\ mass\), "\[IndentingNewLine]", \(\(gamma\ --\) > \ slope\ angle\)}], "Input"] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell["3D Walker Open Chain Kinematics", "Section"], Cell[CellGroupData[{ Cell["Foot/Leg links", "Subsection"], Cell[BoxData[{ \(\(q\ = \ {{x1[t]}, {x2[t]}, {x3[t]}, {x4[t]}, {x5[t]}, {x6[t]}, {x7[ t]}};\)\), "\[IndentingNewLine]", \(\(qdot\ = \ D[q, t];\)\)}], "Input"], Cell[BoxData[{ \(\(ax1\ = \ {1, 0, 0, 0, 0, 0};\)\), "\[IndentingNewLine]", \(\(ax2\ = \ {0, 1, 0, 0, 0, 0};\)\), "\[IndentingNewLine]", \(\(ax3\ = \ {0, 0, 1, 0, 0, 0};\)\), "\[IndentingNewLine]", \(\(ax4\ = \ {0, 0, 0, 0, 0, 1};\)\), "\[IndentingNewLine]", \(\(ax5\ = \ {0, 0, 0, 0, 1, 0};\)\), "\[IndentingNewLine]", \(\(ax6\ = \ {0, 0, 0, 1, 0, 0};\)\), "\[IndentingNewLine]", \(\(ax7\ = \ Flatten[Append[ Cross[{0, 0, l}, {1, 0, 0}], {1, 0, 0}]];\)\)}], "Input"], Cell[BoxData[{ \(\(gst01\ = \ {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, l/2}, {0, 0, 0, 1}};\)\), "\[IndentingNewLine]", \(\(gst02\ = \ {{1, 0, 0, w/2}, {0, 1, 0, 0}, {0, 0, 1, l}, {0, 0, 0, 1}};\)\), "\[IndentingNewLine]", \(\(gst03\ = \ {{1, 0, 0, w}, {0, 1, 0, 0}, {0, 0, 1, l/2}, {0, 0, 0, 1}};\)\)}], "Input"], Cell[CellGroupData[{ Cell[BoxData[{ \(\(J1\ = \ BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {{0, 0, 0, 0, 0, 0}, x7[t]}, \ gst01] // FullSimplify;\)\), "\[IndentingNewLine]", \(\(J2\ = \ BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {{0, 0, 0, 0, 0, 0}, x7[t]}, \ gst02] // FullSimplify;\)\), "\[IndentingNewLine]", \(\(J3\ = \ BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {ax7, x7[t]}, \ gst03] // FullSimplify;\)\), "\[IndentingNewLine]", \(MatrixForm[J1]\), "\[IndentingNewLine]", \(MatrixForm[J2]\), "\[IndentingNewLine]", \(MatrixForm[J3]\)}], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[x4[t]]\ Cos[x5[t]]\), \(Cos[x5[t]]\ Sin[x4[t]]\), \(-Sin[ x5[t]]\), \(1\/2\ l\ Cos[x5[t]]\ Sin[ x6[t]]\), \(1\/2\ l\ Cos[x6[t]]\), "0", "0"}, {\(\(-Cos[x6[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t]]\), \(Cos[x4[t]]\ Cos[ x6[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t]]\), \(Cos[x5[t]]\ Sin[ x6[t]]\), \(1\/2\ l\ Sin[x5[t]]\), "0", \(-\(l\/2\)\), "0"}, {\(Cos[x4[t]]\ Cos[x6[t]]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t]]\), \(Cos[x6[t]]\ Sin[x4[t]]\ Sin[ x5[t]] - Cos[x4[t]]\ Sin[x6[t]]\), \(Cos[x5[t]]\ Cos[x6[t]]\), "0", "0", "0", "0"}, {"0", "0", "0", \(-Sin[x5[t]]\), "0", "1", "0"}, {"0", "0", "0", \(Cos[x5[t]]\ Sin[x6[t]]\), \(Cos[x6[t]]\), "0", "0"}, {"0", "0", "0", \(Cos[x5[t]]\ Cos[x6[t]]\), \(-Sin[x6[t]]\), "0", "0"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[x4[t]]\ Cos[x5[t]]\), \(Cos[x5[t]]\ Sin[x4[t]]\), \(-Sin[ x5[t]]\), \(l\ Cos[x5[t]]\ Sin[x6[t]]\), \(l\ Cos[x6[t]]\), "0", "0"}, {\(\(-Cos[x6[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t]]\), \(Cos[x4[t]]\ Cos[ x6[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t]]\), \(Cos[x5[t]]\ Sin[ x6[t]]\), \(1\/2\ w\ Cos[x5[t]]\ Cos[x6[t]] + l\ Sin[x5[t]]\), \(\(-\(1\/2\)\)\ w\ Sin[x6[t]]\), \(-l\), "0"}, {\(Cos[x4[t]]\ Cos[x6[t]]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t]]\), \(Cos[x6[t]]\ Sin[x4[t]]\ Sin[ x5[t]] - Cos[x4[t]]\ Sin[x6[t]]\), \(Cos[x5[t]]\ Cos[ x6[t]]\), \(\(-\(1\/2\)\)\ w\ Cos[x5[t]]\ Sin[ x6[t]]\), \(\(-\(1\/2\)\)\ w\ Cos[x6[t]]\), "0", "0"}, {"0", "0", "0", \(-Sin[x5[t]]\), "0", "1", "0"}, {"0", "0", "0", \(Cos[x5[t]]\ Sin[x6[t]]\), \(Cos[x6[t]]\), "0", "0"}, {"0", "0", "0", \(Cos[x5[t]]\ Cos[x6[t]]\), \(-Sin[x6[t]]\), "0", "0"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[x4[t]]\ Cos[x5[t]]\), \(Cos[x5[t]]\ Sin[x4[t]]\), \(-Sin[ x5[t]]\), \(\(-\(1\/2\)\)\ l\ Cos[ x5[t]]\ \((\(-2\)\ Sin[x6[t]] + Sin[x6[t] + x7[t]])\)\), \(l\ Cos[x6[t]] - 1\/2\ l\ Cos[x6[t] + x7[t]]\), "0", "0"}, {\(\(-Cos[x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]]\), \(Cos[ x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]]\), \(Cos[ x5[t]]\ Sin[ x6[t] + x7[t]]\), \(w\ Cos[x5[t]]\ Cos[x6[t] + x7[t]] + 1\/2\ l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[ x5[t]]\), \(\(-w\)\ Sin[ x6[t] + x7[t]]\), \(1\/2\ \((l - 2\ l\ Cos[x7[t]])\)\), \(l\/2\)}, {\(Cos[x4[t]]\ Cos[x6[t] + x7[t]]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t] + x7[t]]\), \(Cos[x6[t] + x7[t]]\ Sin[ x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]]\), \(Cos[x5[t]]\ Cos[ x6[t] + x7[t]]\), \(\(-l\)\ Sin[x5[t]]\ Sin[x7[t]] - w\ Cos[x5[t]]\ Sin[x6[t] + x7[t]]\), \(\(-w\)\ Cos[ x6[t] + x7[t]]\), \(l\ Sin[x7[t]]\), "0"}, {"0", "0", "0", \(-Sin[x5[t]]\), "0", "1", "1"}, {"0", "0", "0", \(Cos[x5[t]]\ Sin[x6[t] + x7[t]]\), \(Cos[x6[t] + x7[t]]\), "0", "0"}, {"0", "0", "0", \(Cos[x5[t]]\ Cos[x6[t] + x7[t]]\), \(-Sin[ x6[t] + x7[t]]\), "0", "0"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[BoxData[{ \(\(genM1\ = \ M*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};\)\), "\[IndentingNewLine]", \(\(genM2\ = \ Mp*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};\)\), "\[IndentingNewLine]", \(\(genM3\ = \ M*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};\)\)}], "Input"], Cell[CellGroupData[{ Cell[BoxData[ \(MatrixForm[ Mmatrix\ = \ Transpose[J1] . genM1 . J1\ + \ Transpose[J2] . genM2 . J2\ + \ Transpose[J3] . genM3 . J3\ // FullSimplify]\)], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(2\ M + Mp\), "0", "0", \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]]\ Sin[ x4[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ Cos[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[ x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[ x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((\(-Cos[ x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\)}, {"0", \(2\ M + Mp\), "0", \(1\/2\ \((Cos[ x4[t]]\ \((\((2\ M + Mp)\)\ w\ Cos[x5[t]] + l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) + l\ Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ Sin[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((Cos[ x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\)}, {"0", "0", \(2\ M + Mp\), "0", \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\)}, {\(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]]\ Sin[x4[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ \((Cos[ x4[t]]\ \((\((2\ M + Mp)\)\ w\ Cos[x5[t]] + l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) + l\ Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), "0", \(1\/8\ \((Cos[x5[t]]\^2\ \((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 - l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\) + 4\ l\ \((l\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]]\^2 + w\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[ 2\ x5[t]])\))\)\), \(1\/8\ l\ \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/2\ l\ \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/4\ l\ M\ \((2\ w\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]])\)\)}, {\(1\/2\ Cos[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ Sin[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[ x5[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/8\ l\ \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/8\ \((l\^2\ \ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/2\ l\ w\ \ \((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\)\), \(\(-\(1\/2\)\)\ l\ M\ w\ Sin[ x6[t] + x7[t]]\)}, {\(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\ \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/2\ l\ w\ \((\((2\ M + Mp)\)\ Sin[ x6[t]] - M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\^2\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\), \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\)}, {\(1\/2\ l\ M\ \((\(-Cos[x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\), \(1\/4\ l\ M\ \((2\ w\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(\(-\(1\/2\)\)\ l\ M\ w\ Sin[ x6[t] + x7[t]]\), \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\), \(\(l\^2\ M\)\/4\)} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ \(MatrixForm[Mmatrix\ /. w \[Rule] 0\ // FullSimplify]\)], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(2\ M + Mp\), "0", "0", \(1\/2\ \((l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ Cos[ x4[t]]\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\), \(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[ x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((\(-Cos[ x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\)}, {"0", \(2\ M + Mp\), "0", \(1\/2\ l\ \((Cos[ x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]] + Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[ x4[t]]\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((Cos[ x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\)}, {"0", "0", \(2\ M + Mp\), "0", \(1\/2\ l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]]\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\)}, {\(1\/2\ \((l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]] + Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), "0", \(1\/8\ l\^2\ \((\(-Cos[x5[t]]\^2\)\ \((\(-6\)\ M - 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\) + 4\ \((3\ M + 2\ Mp - 2\ M\ Cos[ x7[t]])\)\ Sin[x5[t]]\^2)\)\), \(1\/8\ l\^2\ \ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\)\), \(1\/2\ l\^2\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]]\), \(1\/4\ l\^2\ M\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]]\)}, {\(1\/2\ l\ Cos[x4[t]]\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[ x4[t]]\), \(1\/2\ l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[ x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[ x5[t]]\), \(1\/8\ l\^2\ Cos[ x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\)\), \(1\/8\ l\^2\ \((6\ M + 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\)\), "0", "0"}, {\(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\^2\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]]\), "0", \(1\/2\ l\^2\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\), \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\)}, {\(1\/2\ l\ M\ \((\(-Cos[x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\), \(1\/4\ l\^2\ M\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]]\), "0", \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\), \(\(l\^2\ M\)\/4\)} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[BoxData[ \(\(KE\ = \ First[First[1/2\ Transpose[qdot] . Mmatrix . qdot]] // Simplify;\)\)], "Input"], Cell[CellGroupData[{ Cell[BoxData[ \(KE /. {x1[t] \[Rule] 0, x2[t] \[Rule] 0, x3[t] \[Rule] 0, \(x1'\)[t] \[Rule] 0, \(x2'\)[t] \[Rule] 0, \(x3'\)[t] \[Rule] 0}\ // FullSimplify\)], "Input"], Cell[BoxData[ RowBox[{\(1\/16\), " ", RowBox[{"(", RowBox[{ RowBox[{\((Cos[x5[t]]\^2\ \((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 - l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\) + 4\ l\ \((l\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]]\^2 + w\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[2\ x5[t]])\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{\((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", "l", " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \((w\ Cos[ x5[t]]\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) + l\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "M", " ", \((2\ w\ Cos[x5[t]]\ Cos[x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"8", " ", "l", " ", "w", " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"M", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"2", " ", \(l\^2\), " ", RowBox[{"(", RowBox[{ RowBox[{\((6\ M + 4\ Mp - 4\ M\ Cos[x7[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", "M", " ", \((1 - 2\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"M", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}]}], ")"}]}]}], ")"}]}]], "Output"] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell["Potential Term", "Subsection"], Cell[CellGroupData[{ Cell[BoxData[{ \(MatrixForm[e1\ = \ TwistExp[ax1, x1[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e2\ = \ TwistExp[ax2, x2[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e3\ = \ TwistExp[ax3, x3[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e4\ = \ TwistExp[ax4, x4[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e5\ = \ TwistExp[ax5, x5[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e6\ = \ TwistExp[ax6, x6[t]]]\), "\[IndentingNewLine]", \(MatrixForm[e7\ = \ TwistExp[ax7, x7[t]]]\)}], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", \(x1[t]\)}, {"0", "1", "0", "0"}, {"0", "0", "1", "0"}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", "0"}, {"0", "1", "0", \(x2[t]\)}, {"0", "0", "1", "0"}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", "0"}, {"0", "1", "0", "0"}, {"0", "0", "1", \(x3[t]\)}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[x4[t]]\), \(-Sin[x4[t]]\), "0", "0"}, {\(Sin[x4[t]]\), \(Cos[x4[t]]\), "0", "0"}, {"0", "0", "1", "0"}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[x5[t]]\), "0", \(Sin[x5[t]]\), "0"}, {"0", "1", "0", "0"}, {\(-Sin[x5[t]]\), "0", \(Cos[x5[t]]\), "0"}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", "0"}, {"0", \(Cos[x6[t]]\), \(-Sin[x6[t]]\), "0"}, {"0", \(Sin[x6[t]]\), \(Cos[x6[t]]\), "0"}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", "0"}, {"0", \(Cos[x7[t]]\), \(-Sin[x7[t]]\), \(l\ Sin[x7[t]]\)}, { "0", \(Sin[x7[t]]\), \(Cos[ x7[t]]\), \(l\ \((1 - Cos[x7[t]])\)\)}, {"0", "0", "0", "1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[BoxData[{ \(\(pS\ = \ {{0}, {0}, {l/2}, {1}};\)\), "\[IndentingNewLine]", \(\(pH\ = \ {{w/2}, {0}, {l}, {1}};\)\), "\[IndentingNewLine]", \(\(pNS\ = {{w}, {0}, {l/2}, {1}};\)\)}], "Input"], Cell[CellGroupData[{ Cell[BoxData[{ \(MatrixForm[ pSp\ = \ e1 . e2 . e3 . e4 . e5 . e6 . pS\ // FullSimplify]\), "\[IndentingNewLine]", \(MatrixForm[ pHp\ = \ e1 . e2 . e3 . e4 . e5 . e6 . pH // FullSimplify]\), "\[IndentingNewLine]", \(MatrixForm[ pNSp\ = \ e1 . e2 . e3 . e4 . e5 . e6 . e7 . pNS\ // FullSimplify]\)}], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(1\/2\ l\ \((Cos[x4[t]]\ Cos[x6[t]]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t]])\) + x1[t]\)}, {\(1\/2\ l\ \((Cos[x6[t]]\ Sin[x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t]])\) + x2[t]\)}, {\(1\/2\ l\ Cos[x5[t]]\ Cos[x6[t]] + x3[t]\)}, {"1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(Cos[ x4[t]]\ \((1\/2\ w\ Cos[x5[t]] + l\ Cos[x6[t]]\ Sin[x5[t]])\) + l\ Sin[x4[t]]\ Sin[x6[t]] + x1[t]\)}, {\(1\/2\ w\ Cos[x5[t]]\ Sin[x4[t]] + l\ Cos[x6[t]]\ Sin[x4[t]]\ Sin[x5[t]] - l\ Cos[x4[t]]\ Sin[x6[t]] + x2[t]\)}, {\(l\ Cos[x5[t]]\ Cos[x6[t]] - 1\/2\ w\ Sin[x5[t]] + x3[t]\)}, {"1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"], Cell[BoxData[ RowBox[{\(General::"spell"\), \(\(:\)\(\ \)\), "\<\"Possible spelling \ error: new symbol name \\\"\\!\\(pNSp\\)\\\" is similar to existing symbols \ \\!\\({pNS, pSp}\\). \\!\\(\\*ButtonBox[\\\"More\[Ellipsis]\\\", \ ButtonStyle->\\\"RefGuideLinkText\\\", ButtonFrame->None, \ ButtonData:>\\\"General::spell\\\"]\\)\"\>"}]], "Message"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(1\/2\ \((Cos[ x4[t]]\ \((2\ w\ Cos[x5[t]] - l\ \((\(-2\)\ Cos[x6[t]] + Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) - l\ Sin[x4[t]]\ \((\(-2\)\ Sin[x6[t]] + Sin[x6[t] + x7[t]])\) + 2\ x1[t])\)\)}, {\(1\/2\ \((2\ w\ Cos[x5[t]]\ Sin[x4[t]] - l\ \((\(-2\)\ Cos[x6[t]] + Cos[x6[t] + x7[t]])\)\ Sin[ x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\(-2\)\ Sin[x6[t]] + Sin[x6[t] + x7[t]])\) + 2\ x2[t])\)\)}, {\(\(-\(1\/2\)\)\ l\ Cos[ x5[t]]\ \((\(-2\)\ Cos[x6[t]] + Cos[x6[t] + x7[t]])\) - w\ Sin[x5[t]] + x3[t]\)}, {"1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ \(PE\ = \ First[M*g*pSp[\([3]\)] + Mp*g*pHp[\([3]\)] + M*g*pNSp[\([3]\)]] // FullSimplify\)], "Input"], Cell[BoxData[ \(1\/2\ g\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[x5[t]] + 2\ \((2\ M + Mp)\)\ x3[t])\)\)], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ \(% /. w \[Rule] 0\)], "Input"], Cell[BoxData[ \(1\/2\ g\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) + 2\ \((2\ M + Mp)\)\ x3[t])\)\)], "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell["3D Walker Dynamics", "Section"], Cell[BoxData[ \(\(Lagrangian\ = \ KE\ - \ PE\ /. {x1[t] \[Rule] 0, x2[t] \[Rule] 0, x3[t] \[Rule] 0, \(x1'\)[t] \[Rule] 0, \(x2'\)[t] \[Rule] 0, \(x3'\)[t] \[Rule] 0}\ ;\)\)], "Input"], Cell[CellGroupData[{ Cell[BoxData[{ \(eq1\ = \ D[D[Lagrangian, \(x4'\)[t]], t]\ - \ D[Lagrangian, x4[t]]\ // FullSimplify\), "\[IndentingNewLine]", \(eq2\ = \ D[D[Lagrangian, \(x5'\)[t]], t]\ - \ D[Lagrangian, x5[t]]\ // FullSimplify\), "\[IndentingNewLine]", \(eq3\ = \ D[D[Lagrangian, \(x6'\)[t]], t]\ - \ D[Lagrangian, x6[t]]\ // FullSimplify\), "\[IndentingNewLine]", \(eq4\ = \ D[D[Lagrangian, \(x7'\)[t]], t]\ - \ D[Lagrangian, x7[t]]\ // FullSimplify\)}], "Input"], Cell[BoxData[ RowBox[{\(1\/8\), " ", RowBox[{"(", RowBox[{ RowBox[{\(-l\), " ", \((4\ w\ Cos[ x5[t]]\ \((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\) + l\ Sin[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "4", " ", "l", " ", "Mp", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "8", " ", \(l\^2\), " ", "M", " ", \(Sin[x5[t]]\), " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Sin[x5[t]]\), " ", \(Sin[x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", \(l\^2\), " ", \(Cos[x5[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((\(-6\)\ M - 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "M", " ", \((\(-1\) + 2\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 2\ Cos[2\ x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((8\ l\ w\ Cos[ 2\ x5[t]]\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[ x6[t] + x7[t]])\) + \((l\^2\ \((6\ M + 4\ Mp)\) - 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[ 2\ x6[t] + x7[t]])\))\))\)\ Sin[ 2\ x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", "l", " ", RowBox[{"(", RowBox[{ RowBox[{\((2\ w\ Sin[ 2\ x5[ t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[ x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\^2\ \((\((5\ M + 4\ Mp)\)\ Sin[ 2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "M", " ", \((4\ l\ Sin[x5[t]]\^2\ Sin[x7[t]] + 2\ w\ Sin[2\ x5[t]]\ Sin[x6[t] + x7[t]] + l\ Cos[x5[t]]\^2\ \((2\ Sin[x7[t]] + Sin[2\ \((x6[t] + x7[t])\)] - 2\ Sin[2\ x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}], "+", RowBox[{"6", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", \(l\^2\), " ", "Mp", " ", \(Cos[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"8", " ", "M", " ", \(w\^2\), " ", \(Cos[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "Mp", " ", \(w\^2\), " ", \(Cos[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "5", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\^2\), " ", \(Cos[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "Mp", " ", \(Cos[x5[t]]\^2\), " ", \(Cos[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\^2\), " ", \(Cos[x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(l\^2\), " ", "M", " ", \(Cos[x5[t]]\^2\), " ", \(Cos[2\ \((x6[t] + x7[t])\)]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\^2\), " ", \(Cos[2\ x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "12", " ", \(l\^2\), " ", "M", " ", \(Sin[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "8", " ", \(l\^2\), " ", "Mp", " ", \(Sin[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "8", " ", \(l\^2\), " ", "M", " ", \(Cos[x7[t]]\), " ", \(Sin[x5[t]]\^2\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Cos[x6[t]]\), " ", \(Sin[2\ x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", "l", " ", "Mp", " ", "w", " ", \(Cos[x6[t]]\), " ", \(Sin[2\ x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Cos[x6[t] + x7[t]]\), " ", \(Sin[2\ x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Sin[x5[t]]\), " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "Mp", " ", "w", " ", \(Sin[x5[t]]\), " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "5", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\), " ", \(Sin[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", \(l\^2\), " ", "Mp", " ", \(Cos[x5[t]]\), " ", \(Sin[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Sin[x5[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(l\^2\), " ", "M", " ", \(Cos[x5[t]]\), " ", \(Sin[2\ \((x6[t] + x7[t])\)]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Cos[x5[t]]\), " ", \(Sin[2\ x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "Mp", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"12", " ", \(l\^2\), " ", "M", " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"8", " ", \(l\^2\), " ", "Mp", " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "8", " ", \(l\^2\), " ", "M", " ", \(Cos[x7[t]]\), " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "l", " ", "M", " ", \((2\ w\ Cos[x5[t]]\ Cos[x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]], "Output"], Cell[BoxData[ RowBox[{\(1\/16\), " ", RowBox[{"(", RowBox[{\(\(-8\)\ g\ \((2\ M + Mp)\)\ w\ Cos[x5[t]]\), "+", \(8\ g\ l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]]\), "-", RowBox[{\((8\ l\ w\ Cos[ 2\ x5[t]]\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) + \((l\^2\ \((6\ M + 4\ Mp)\) - 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\)\ Sin[ 2\ x5[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"4", " ", "l", " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((l\ Cos[ x5[t]]\ \((6\ M + 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\) + 4\ w\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "M", " ", \((l\ Cos[ x5[t]]\ \((1 - 2\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 2\ Cos[2\ x6[t] + x7[t]])\) + 4\ w\ Cos[x6[t] + x7[t]]\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ "4", " ", "l", " ", "w", " ", \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Cos[x6[t] + x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", \(l\^2\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\(-\((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\)\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "M", " ", \((2\ Sin[x7[t]] - Sin[2\ \((x6[t] + x7[t])\)] + 2\ Sin[2\ x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ "4", " ", "l", " ", "w", " ", \(Sin[x5[t]]\), " ", \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(l\^2\), " ", \(Cos[x5[t]]\), " ", \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"6", " ", \(l\^2\), " ", "M", " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"4", " ", \(l\^2\), " ", "Mp", " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"8", " ", "M", " ", \(w\^2\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", "Mp", " ", \(w\^2\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "5", " ", \(l\^2\), " ", "M", " ", \(Cos[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", \(l\^2\), " ", "Mp", " ", \(Cos[2\ x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Cos[x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(l\^2\), " ", "M", " ", \(Cos[2\ \((x6[t] + x7[t])\)]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", \(l\^2\), " ", "M", " ", \(Cos[2\ x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "8", " ", "l", " ", "M", " ", "w", " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", "l", " ", "Mp", " ", "w", " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", "M", " ", "w", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}]], "Output"], Cell[BoxData[ RowBox[{\(-\(1\/8\)\), " ", "l", " ", RowBox[{"(", RowBox[{ RowBox[{\((2\ w\ Sin[ 2\ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\^2\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "l", " ", \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((l\ Cos[ x5[t]]\ \((6\ M + 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\) + 4\ w\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "4", " ", "l", " ", "M", " ", \(Sin[x5[t]]\), " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"4", " ", RowBox[{"(", RowBox[{\(g\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\)\), "-", RowBox[{"2", " ", "l", " ", "M", " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"l", " ", "M", " ", \(Sin[x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "2", " ", "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "Mp", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "M", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"3", " ", "l", " ", "M", " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", "l", " ", "Mp", " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "2", " ", "l", " ", "M", " ", \(Cos[x7[t]]\), " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"2", " ", "M", " ", "w", " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"Mp", " ", "w", " ", \(Sin[x6[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"M", " ", "w", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "l", " ", \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ "2", " ", "l", " ", "M", " ", \((\(-1\) + 2\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]], "Output"], Cell[BoxData[ RowBox[{\(-\(1\/8\)\), " ", "l", " ", "M", " ", RowBox[{"(", RowBox[{ RowBox[{\((4\ l\ Sin[x5[t]]\^2\ Sin[x7[t]] + 2\ w\ Sin[2\ x5[t]]\ Sin[x6[t] + x7[t]] + l\ Cos[x5[t]]\^2\ \((2\ Sin[x7[t]] + Sin[2\ \((x6[t] + x7[t])\)] - 2\ Sin[2\ x6[t] + x7[t]])\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "l", " ", \((2\ Sin[x7[t]] - Sin[2\ \((x6[t] + x7[t])\)] + 2\ Sin[2\ x6[t] + x7[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"2", " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\((l\ Cos[ x5[t]]\ \((1 - 2\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 2\ Cos[2\ x6[t] + x7[t]])\) + 4\ w\ Cos[x6[t] + x7[t]]\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ "4", " ", "l", " ", \(Sin[x5[t]]\), " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ "2", " ", "w", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{"(", RowBox[{\(g\ Cos[x5[t]]\), "-", RowBox[{"w", " ", RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"l", " ", RowBox[{"(", RowBox[{ RowBox[{\(-2\), " ", \(Sin[x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{\((\(-1\) + 2\ Cos[x7[t]])\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["x7", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], ")"}]}]], "Output"] }, Open ]], Cell[BoxData[ \(\(Solve[{eq1\ \[Equal] 0\ , \ eq2 \[Equal] 0, eq3 \[Equal] 0, eq4 \[Equal] 0}, {\(x4''\)[t], \(x5''\)[t], \(x6''\)[t], \(x7''\)[ t]}];\)\)], "Input"], Cell[CellGroupData[{ Cell["Without a hip:", "Subsection"], Cell[CellGroupData[{ Cell[BoxData[ \(Solve[{eq1\ \[Equal] 0\ /. w \[Rule] 0, \ eq2 \[Equal] 0\ /. w \[Rule] 0, eq3 \[Equal] 0\ /. w \[Rule] 0, eq4 \[Equal] 0\ /. w \[Rule] 0}, {\(x4''\)[t], \(x5''\)[ t], \(x6''\)[t], \(x7''\)[t]}]\ // FullSimplify\)], "Input"], Cell[BoxData[ RowBox[{"{", RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ SuperscriptBox["x5", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{\(1\/\(l\ \((M + 4\ Mp)\)\)\), RowBox[{"(", RowBox[{\(Csc[x7[t]]\), " ", RowBox[{"(", RowBox[{\(2\ g\ Sin[ x5[t]]\ \((\(-M\)\ Sin[ x6[t]] + \((M + 2\ Mp)\)\ Sin[ x6[t] + x7[t]])\)\), "+", RowBox[{"l", " ", \((M + 4\ Mp)\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Cos[x5[t]]\), " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{"2", " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{"2", " ", \(Sin[x6[t]]\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}], ")"}]}]}], ",", RowBox[{ RowBox[{ SuperscriptBox["x4", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{\(1\/\(l\ \((M + 4\ Mp)\)\)\), RowBox[{"(", RowBox[{"2", " ", \(Csc[x7[t]]\), " ", \(Sec[x5[t]]\), " ", RowBox[{"(", RowBox[{\(g\ \((M\ Cos[x6[t]] - \((M + 2\ Mp)\)\ Cos[ x6[t] + x7[t]])\)\ Sin[x5[t]]\), "+", RowBox[{"l", " ", \((M + 4\ Mp)\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(Cos[x6[t]]\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}], ")"}]}]}], ",", RowBox[{ RowBox[{ SuperscriptBox["x6", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{"-", RowBox[{ RowBox[{"(", RowBox[{\(Csc[x7[t]]\), " ", \(Sec[x5[t]]\), " ", RowBox[{"(", RowBox[{\(16\ g\ \((Cos[ x6[t]]\ \((\((\(-5\)\ M\^2 - 20\ M\ Mp - 16\ Mp\^2 + \((3\ M\^2 + 12\ M\ Mp + 16\ Mp\^2)\)\ Cos[2\ x5[t]])\)\ Cos[ x7[t]] - M\ \((\(-3\)\ M - 8\ Mp + M\ Cos[2\ x5[t]])\)\ Cos[3\ x7[t]] + 4\ M\ \((3\ M + 4\ Mp - 2\ M\ Cos[ 2\ x7[t]])\)\ Sin[x5[t]]\^2)\) + Sin[x6[ t]]\ \((\((13\ M\^2 + 48\ M\ Mp + 32\ Mp\^2 - 3\ M\^2\ Cos[2\ x5[t]])\)\ Sin[ x7[t]] + M\ \((\(-3\)\ M - 8\ Mp + M\ Cos[2\ x5[t]])\)\ Sin[ 3\ x7[t]])\))\)\), "+", RowBox[{"8", " ", "l", " ", \((M + 4\ Mp)\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x7[t]]\), " ", \((M\ Cos[ 3\ x5[t]]\ \((Sin[x7[t]] - Sin[2\ x7[t]])\) + 5\ M\ Cos[ x5[t]]\ \((\(-Sin[x7[t]]\) + Sin[2\ x7[t]])\) + Cos[x5[t]]\^3\ \((\((6\ M + 8\ Mp)\)\ Sin[ 2\ x6[t]] + 4\ M\ \((Cos[ 2\ \((x6[t] + x7[t])\)]\ Sin[x7[t]] - Sin[2\ \((x6[t] + x7[t])\)])\))\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", RowBox[{ "2", " ", \(Cos[x5[t]]\), " ", \(Sin[x7[t]]\), " ", \((\((3\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + 4\ M\ \((Cos[x6[t] + x7[t]]\^2\ Sin[ x7[t]] - Cos[x6[t]]\ Sin[ x6[t] + 2\ x7[t]])\))\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"4", " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x7[t]]\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Cos[x5[t]]\^2\), " ", \((3\ M + 4\ Mp + \((3\ M + 4\ Mp)\)\ Cos[ 2\ x6[t]] - 2\ M\ \((Cos[2\ x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] + Sin[x7[t]]\ Sin[2\ \((x6[t] + x7[t])\)])\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "M", " ", \((1 - 2\ Cos[x7[t]])\), " ", \(Sin[2\ x5[t]]\), " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{\(1\/2\), " ", \(Sin[2\ x5[t]]\), " ", \((2\ Cos[x6[t]]\^2\ Cos[ x7[t]]\ \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\) + Sin[x7[t]]\ \((\((3\ M + 4\ Mp - 2\ M\ Cos[2\ x7[t]])\)\ Sin[ 2\ x6[t]] + 4\ M\ Sin[x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "-", RowBox[{ "8", " ", \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\), " ", \(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(Cos[x6[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ "8", " ", "M", " ", \(Cos[x5[t]]\), " ", \(Sin[x7[t]]\^2\), " ", RowBox[{"(", RowBox[{ RowBox[{"2", " ", \(Cos[x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", SuperscriptBox[ RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}], ")"}]}]}], ")"}]}], ")"}], "/", \((32\ l\ \((M + 4\ Mp)\)\ \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\))\)}]}]}], ",", RowBox[{ RowBox[{ SuperscriptBox["x7", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{\(1\/\(l\ \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\)\)\), RowBox[{"(", RowBox[{\(2\ g\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[ x6[t]] + \((5\ M + 4\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x6[t] + x7[t]])\)\), "+", RowBox[{ "l", " ", \((\(-9\)\ M - 6\ Mp + 6\ M\ Cos[x7[t]] + Cos[2\ x5[t]]\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\) + Cos[x5[t]]\^2\ \((M\ Cos[ 2\ \((x6[t] + x7[t])\)] + \((5\ M + 4\ Mp)\)\ \((Cos[2\ x6[t]] - Cos[2\ x6[t] + x7[t]])\))\))\), " ", \(Sin[x7[t]]\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{"l", " ", \(Sin[x7[t]]\), " ", RowBox[{"(", RowBox[{ RowBox[{\(-\((6\ M + 4\ Mp + \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] - 4\ M\ Cos[x7[t]] + M\ Cos[ 2\ \((x6[t] + x7[t])\)] - \((5\ M + 4\ Mp)\)\ Cos[2\ x6[t] + x7[t]])\)\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "4", " ", \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ "4", " ", "M", " ", \((\(-1\) + 2\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "M", " ", \((\(-1\) + 2\ Cos[x7[t]])\), " ", SuperscriptBox[ RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], "2"]}]}], ")"}]}], "+", RowBox[{"2", " ", "l", " ", \(Sin[x7[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{\(-Cos[x5[t]]\), " ", \((M\ Sin[ 2\ \((x6[t] + x7[t])\)] + \((5\ M + 4\ Mp)\)\ \((Sin[2\ x6[t]] - Sin[2\ x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", \(Sin[x5[t]]\), " ", RowBox[{"(", RowBox[{ RowBox[{\((6\ M + 4\ Mp - 4\ M\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "M", " ", \((1 - 2\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}]}]}], "}"}], "}"}]], "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell["Impact Equations", "Section"], Cell[CellGroupData[{ Cell[BoxData[{ \(\(pNSFoot\ = {{w}, {0}, {0}, {1}};\)\), "\[IndentingNewLine]", \(MatrixForm[ pNSFootp\ = \ e1 . e2 . e3 . e4 . e5 . e6 . e7 . pNSFoot\ // FullSimplify]\)}], "Input"], Cell[BoxData[ RowBox[{\(General::"spell1"\), \(\(:\)\(\ \)\), "\<\"Possible spelling \ error: new symbol name \\\"\\!\\(pNSFootp\\)\\\" is similar to existing \ symbol \\\"\\!\\(pNSFoot\\)\\\". \\!\\(\\*ButtonBox[\\\"More\[Ellipsis]\\\", \ ButtonStyle->\\\"RefGuideLinkText\\\", ButtonFrame->None, \ ButtonData:>\\\"General::spell1\\\"]\\)\"\>"}]], "Message"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(w\ Cos[x4[t]]\ Cos[x5[t]] - 2\ l\ \((Cos[x6[t] + x7[t]\/2]\ Sin[x4[t]] - Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2] + x1[t]\)}, {\(w\ Cos[x5[t]]\ Sin[x4[t]] + 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2] + x2[t]\)}, {\(\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[x7[t]\/2] + x3[t]\)}, {"1"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[{ \(\(Pos1\ = \ First[pNSFootp[\([1]\)]];\)\), "\[IndentingNewLine]", \(\(Pos2\ = \ First[pNSFootp[\([2]\)]];\)\), "\[IndentingNewLine]", \(\(Pos3\ = \ First[pNSFootp[\([3]\)]];\)\), "\[IndentingNewLine]", \(MatrixForm[ F\ = \ FullSimplify[{{D[Pos1, x1[t]], D[Pos1, x2[t]], D[Pos1, x3[t]], D[Pos1, x4[t]], D[Pos1, x5[t]], D[Pos1, x6[t]], D[Pos1, x7[t]]}, \[IndentingNewLine]{D[Pos2, x1[t]], D[Pos2, x2[t]], D[Pos2, x3[t]], D[Pos2, x4[t]], D[Pos2, x5[t]], D[Pos2, x6[t]], D[Pos2, x7[t]]}, \[IndentingNewLine]{D[Pos3, x1[t]], D[Pos3, x2[t]], D[Pos3, x3[t]], D[Pos3, x4[t]], D[Pos3, x5[t]], D[Pos3, x6[t]], D[Pos3, x7[t]]}}]]\)}], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", "0", \(\(-w\)\ Cos[x5[t]]\ Sin[x4[t]] - 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(Cos[ x4[t]]\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(2\ l\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-l\)\ Cos[x6[t] + x7[t]]\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]]\)}, {"0", "1", "0", \(w\ Cos[x4[t]]\ Cos[x5[t]] + 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(Sin[ x4[t]]\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(2\ l\ \((Cos[x6[t] + x7[t]\/2]\ Sin[ x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\)}, {"0", "0", "1", "0", \(\(-w\)\ Cos[x5[t]] - 2\ l\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2]\), \(2\ l\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\), \(l\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\)} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ \(MatrixForm[ nFT\ = \ Join[\(-Transpose[F]\), {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}]]\)], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(-1\), "0", "0"}, {"0", \(-1\), "0"}, {"0", "0", \(-1\)}, {\(w\ Cos[x5[t]]\ Sin[x4[t]] + 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-w\)\ Cos[x4[t]]\ Cos[x5[t]] - 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), "0"}, {\(\(-Cos[x4[t]]\)\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(\(-Sin[ x4[t]]\)\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(w\ Cos[x5[t]] + 2\ l\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\)}, {\(\(-2\)\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-2\)\ l\ \((Cos[x6[t] + x7[t]\/2]\ Sin[ x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-2\)\ l\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\)}, {\(l\ Cos[x6[t] + x7[t]]\ Sin[x4[t]] - l\ Cos[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]]\), \(\(-l\)\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(\(-l\)\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\)}, {"0", "0", "0"}, {"0", "0", "0"}, {"0", "0", "0"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ \(MatrixForm[ FullMatrix\ = \ Transpose[ Join[Transpose[Join[Mmatrix, F]], Transpose[nFT]]]]\)], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {\(2\ M + Mp\), "0", "0", \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]]\ Sin[ x4[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ Cos[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[ x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[ x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((\(-Cos[ x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\), \(-1\), "0", "0"}, {"0", \(2\ M + Mp\), "0", \(1\/2\ \((Cos[ x4[t]]\ \((\((2\ M + Mp)\)\ w\ Cos[x5[t]] + l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) + l\ Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ Sin[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ M\ \((Cos[ x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\), "0", \(-1\), "0"}, {"0", "0", \(2\ M + Mp\), "0", \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\), "0", "0", \(-1\)}, {\(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]]\ Sin[x4[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[x5[t]] + l\ Cos[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ \((Cos[ x4[t]]\ \((\((2\ M + Mp)\)\ w\ Cos[x5[t]] + l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) + l\ Sin[x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\)\), "0", \(1\/8\ \((Cos[x5[t]]\^2\ \((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 - l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\) + 4\ l\ \((l\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]]\^2 + w\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[ 2\ x5[t]])\))\)\), \(1\/8\ l\ \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/2\ l\ \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/4\ l\ M\ \((2\ w\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(w\ Cos[x5[t]]\ Sin[x4[t]] + 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-w\)\ Cos[x4[t]]\ Cos[x5[t]] - 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), "0"}, {\(1\/2\ Cos[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ Sin[ x4[t]]\ \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[ x5[t]])\)\), \(1\/2\ \((\(-\((2\ M + Mp)\)\)\ w\ Cos[ x5[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/8\ l\ \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/8\ \((l\^2\ \ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[ 2\ x6[t] + x7[t]])\))\))\)\), \(1\/2\ l\ w\ \ \((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\)\), \(\(-\(1\/2\)\)\ l\ M\ w\ Sin[ x6[t] + x7[t]]\), \(\(-Cos[ x4[t]]\)\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(\(-Sin[ x4[t]]\)\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(w\ Cos[x5[t]] + 2\ l\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\)}, {\(1\/2\ \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\)\), \(1\/2\ l\ Cos[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\ \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(1\/2\ l\ w\ \((\((2\ M + Mp)\)\ Sin[ x6[t]] - M\ Sin[x6[t] + x7[t]])\)\), \(1\/2\ l\^2\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\), \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\), \(\(-2\)\ l\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-2\)\ l\ \((Cos[x6[t] + x7[t]\/2]\ Sin[ x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-2\)\ l\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\)}, {\(1\/2\ l\ M\ \((\(-Cos[x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(1\/2\ l\ M\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\), \(1\/4\ l\ M\ \((2\ w\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[ x5[t]])\)\), \(\(-\(1\/2\)\)\ l\ M\ w\ Sin[ x6[t] + x7[t]]\), \(1\/4\ l\ M\ \((l - 2\ l\ Cos[x7[t]])\)\), \(\(l\^2\ M\)\/4\), \(l\ Cos[ x6[t] + x7[t]]\ Sin[x4[t]] - l\ Cos[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]]\), \(\(-l\)\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[ x6[t] + x7[t]])\)\), \(\(-l\)\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\)}, {"1", "0", "0", \(\(-w\)\ Cos[x5[t]]\ Sin[x4[t]] - 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(Cos[ x4[t]]\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(2\ l\ \((Cos[x4[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x5[t]] + Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(\(-l\)\ Cos[x6[t] + x7[t]]\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]]\), "0", "0", "0"}, {"0", "1", "0", \(w\ Cos[x4[t]]\ Cos[x5[t]] + 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(Sin[ x4[t]]\ \((\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2])\)\), \(2\ l\ \((Cos[x6[t] + x7[t]\/2]\ Sin[ x4[t]]\ Sin[x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2]\), \(l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\)\), "0", "0", "0"}, {"0", "0", "1", "0", \(\(-w\)\ Cos[x5[t]] - 2\ l\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2]\), \(2\ l\ Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\), \(l\ Cos[x5[t]]\ Sin[ x6[t] + x7[t]]\), "0", "0", "0"} }, RowSpacings->1, ColumnSpacings->1, ColumnAlignments->{Left}], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell[BoxData[ \(\(MatrixForm[FullMatrix /. w \[Rule] 0 // FullSimplify];\)\)], "Input"], Cell[CellGroupData[{ Cell[BoxData[ \(MatrixForm[ DMatrix\ = \ Join[Mmatrix . {0, 0, 0, \(x4'\)[t], \(x5'\)[t], \(x6'\)[t], \(x7'\)[t]}, {0, 0, 0}]]\)], "Input"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", TagBox[GridBox[{ { RowBox[{ RowBox[{\(1\/2\), " ", \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]]\ Sin[x4[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]]\ Sin[ x5[t]] + l\ Cos[ x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", \(Cos[x4[t]]\), " ", \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", \((l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x4[t]] + l\ Cos[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", "M", " ", \((\(-Cos[x6[t] + x7[t]]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/2\), " ", \((Cos[ x4[t]]\ \((\((2\ M + Mp)\)\ w\ Cos[x5[t]] + l\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\) + l\ Sin[ x4[t]]\ \((\((3\ M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", \(Sin[x4[t]]\), " ", \((l\ Cos[ x5[t]]\ \((\((3\ M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\) - \((2\ M + Mp)\)\ w\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", \((Cos[ x4[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + Sin[x4[t]]\ Sin[ x5[t]]\ \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\))\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", "M", " ", \((Cos[x4[t]]\ Cos[x6[t] + x7[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/2\), " ", \((\(-\((2\ M + Mp)\)\)\ w\ Cos[x5[t]] + l\ \((\(-\((3\ M + 2\ Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", \(Cos[x5[t]]\), " ", \((\(-\((3\ M + 2\ Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", "M", " ", \(Cos[x5[t]]\), " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/8\), " ", \((Cos[x5[t]]\^2\ \((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 - l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((4\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\) + 4\ l\ \((l\ \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\)\ Sin[x5[t]]\^2 + w\ \((\((2\ M + Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\)\ Sin[ 2\ x5[t]])\))\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/8\), " ", "l", " ", \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[ x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/4\), " ", "l", " ", "M", " ", \((2\ w\ Cos[x5[t]]\ Cos[x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/8\), " ", "l", " ", \((4\ w\ Sin[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Sin[x6[t]] + M\ Sin[x6[t] + x7[t]])\) + l\ Cos[ x5[t]]\ \((\((5\ M + 4\ Mp)\)\ Sin[2\ x6[t]] + M\ \((Sin[2\ \((x6[t] + x7[t])\)] - 4\ Sin[2\ x6[t] + x7[t]])\))\))\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/8\), " ", \((l\^2\ \((6\ M + 4\ Mp)\) + 2\ \((4\ M + Mp)\)\ w\^2 + l\^2\ \((\((5\ M + 4\ Mp)\)\ Cos[2\ x6[t]] + M\ \((\(-4\)\ Cos[x7[t]] + Cos[2\ \((x6[t] + x7[t])\)] - 4\ Cos[2\ x6[t] + x7[t]])\))\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", "w", " ", \((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(1\/2\), " ", "l", " ", "M", " ", "w", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/2\), " ", "l", " ", \((w\ Cos[ x5[t]]\ \((\(-\((2\ M + Mp)\)\)\ Cos[x6[t]] + M\ Cos[x6[t] + x7[t]])\) + l\ \((\(-3\)\ M - 2\ Mp + 2\ M\ Cos[x7[t]])\)\ Sin[ x5[t]])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", "l", " ", "w", " ", \((\((2\ M + Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/2\), " ", \(l\^2\), " ", \((3\ M + 2\ Mp - 2\ M\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/4\), " ", "l", " ", "M", " ", \((l - 2\ l\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, { RowBox[{ RowBox[{\(1\/4\), " ", "l", " ", "M", " ", \((2\ w\ Cos[x5[t]]\ Cos[x6[t] + x7[t]] + l\ \((\(-1\) + 2\ Cos[x7[t]])\)\ Sin[x5[t]])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\(1\/2\), " ", "l", " ", "M", " ", "w", " ", \(Sin[x6[t] + x7[t]]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/4\), " ", "l", " ", "M", " ", \((l - 2\ l\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\(1\/4\), " ", \(l\^2\), " ", "M", " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}, {"0"}, {"0"}, {"0"} }, RowSpacings->1, ColumnAlignments->{Left}], Column], "\[NoBreak]", ")"}], Function[ BoxForm`e$, MatrixForm[ BoxForm`e$]]]], "Output"] }, Open ]], Cell["First, we have the angular positions for 4,5,6,7:", "Text"], Cell[BoxData[ \(\({x4impact\ = \ \[Pi]\ + \ x4[t], \ x5impact\ = \ x5[t], \ x6impact\ = \ x6[t] + x7[t], \ x7impact\ = \ \(-x7[t]\)};\)\)], "Input"], Cell["\<\ We are interested in the angular velocities for 4,5,6,7 (as of now these are \ incomputable for the hipped walker):\ \>", "Text"], Cell[BoxData[ \(\(K\ = \ Inverse[FullMatrix] . DMatrix;\)\)], "Input"], Cell[CellGroupData[{ Cell[BoxData[{ \(x4dotimpact\ = \ FullSimplify[K[\([4]\)]]\), "\[IndentingNewLine]", \(x5dotimpact\ = \ FullSimplify[K[\([5]\)]]\), "\n", \(x6dotimpact\ = \ FullSimplify[K[\([6]\)] + K[\([7]\)]]\), "\n", \(x7dotimpact\ = \ \(-FullSimplify[K[\([7]\)]]\)\)}], "Input"], Cell[BoxData[ \($Aborted\)], "Output"], Cell[BoxData[ \($Aborted\)], "Output"], Cell[BoxData[ \($Aborted\)], "Output"], Cell[BoxData[ \($Aborted\)], "Output"] }, Open ]], Cell[CellGroupData[{ Cell["Without Hip", "Subsection"], Cell[BoxData[{ \(\(FullMatrix\ = \ FullMatrix\ /. \ w \[Rule] 0\ // FullSimplify;\)\), "\[IndentingNewLine]", \(\(DMatrix\ = \ DMatrix\ /. \ w \[Rule] 0\ // FullSimplify;\)\), "\[IndentingNewLine]", \(\(K\ = \ Inverse[FullMatrix] . DMatrix;\)\)}], "Input"], Cell[CellGroupData[{ Cell[BoxData[{ \(x4dotimpact\ = \ FullSimplify[K[\([4]\)]]\), "\[IndentingNewLine]", \(x5dotimpact\ = \ FullSimplify[K[\([5]\)]]\), "\n", \(x6dotimpact\ = \ FullSimplify[K[\([6]\)] + K[\([7]\)]]\), "\n", \(x7dotimpact\ = \ \(-FullSimplify[K[\([7]\)]]\)\)}], "Input"], Cell[BoxData[ FractionBox[ RowBox[{\(Sec[x7[t]\/2]\), " ", RowBox[{"(", RowBox[{ RowBox[{\((M\ Cos[2\ x6[t] + \(3\ x7[t]\)\/2] + 2\ \((M + 2\ Mp)\)\ Sin[x6[t]]\ Sin[x6[t] + x7[t]\/2])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", \((\((M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\), " ", \(Sec[x5[t]]\), " ", \(Sin[x6[t] + x7[t]\/2]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], \(M + 4\ Mp\)]], "Output"], Cell[BoxData[ FractionBox[ RowBox[{\(Sec[x7[t]\/2]\), " ", RowBox[{"(", RowBox[{ RowBox[{ "2", " ", \(Cos[x5[t]]\), " ", \(Cos[x6[t] + x7[t]\/2]\), " ", \((\((M + 2\ Mp)\)\ Sin[x6[t]] - M\ Sin[x6[t] + x7[t]])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\((2\ \((M + 2\ Mp)\)\ Cos[x6[t]]\ Cos[ x6[t] + x7[t]\/2] - M\ Cos[2\ x6[t] + \(3\ x7[t]\)\/2])\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], \(M + 4\ Mp\)]], "Output"], Cell[BoxData[ RowBox[{\(1\/\(2\ \((M + 4\ Mp)\)\ \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\)\)\), RowBox[{"(", RowBox[{\(Sec[x5[t]]\), " ", \(Sec[x7[t]\/2]\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[2\ x5[t]]\), " ", \((\((M + 2\ Mp)\)\ \((2\ \((M + 2\ Mp)\)\ Cos[\(3\ x7[t]\)\/2] + M\ Cos[\(5\ x7[t]\)\/2])\) + Cos[x7[t]\/2]\ \((\(-M\)\ \((3\ M + 8\ Mp)\) + 2\ Cos[ 2\ x6[t]]\ \((\((M + Mp)\)\ \((3\ M + 4\ Mp)\) + M\ \((\(-2\)\ \((M + 2\ Mp)\)\ Cos[x7[t]] - 2\ \((M + Mp)\)\ Cos[2\ x7[t]] + M\ Cos[3\ x7[t]])\))\))\) - 2\ \((Mp\ \((3\ M + 4\ Mp)\) + M\ \((\(-2\)\ \((M + 2\ Mp)\)\ Cos[x7[t]] - 2\ Mp\ Cos[2\ x7[t]] + M\ Cos[3\ x7[t]])\))\)\ Sin[2\ x6[t]]\ Sin[ x7[t]\/2])\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ "2", " ", \((\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]])\), " ", \((\((M + 2\ Mp)\)\ Cos[x6[t]] - M\ Cos[x6[t] + x7[t]])\), " ", \(Sin[x5[t]]\), " ", \(Sin[x6[t] + x7[t]\/2]\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{\((M + 4\ Mp)\), " ", \(Cos[x5[t]]\), " ", \(Cos[x7[t]\/2]\), " ", RowBox[{"(", RowBox[{ RowBox[{\((M - 2\ \((M + 2\ Mp)\)\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{"M", " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}], ")"}]}]], "Output"], Cell[BoxData[ RowBox[{"-", FractionBox[ RowBox[{ RowBox[{ "8", " ", \((M + Mp)\), " ", \((1 + 2\ Cos[x7[t]])\), " ", \(Sin[x7[t]\/2]\^2\), " ", RowBox[{"(", RowBox[{ RowBox[{\(Sin[x5[t]]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{"M", " ", \((1 - 2\ Cos[x7[t]])\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], \(\(-3\)\ M - 4\ Mp + 2\ M\ Cos[2\ x7[t]]\)]}]], "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell["Collision Guard", "Section"], Cell["Note: gamma is the slope angle", "Text"], Cell[CellGroupData[{ Cell[BoxData[ \(height\ = \ First[pNSFootp[\([3]\)]\ + \ Tan[gamma]*pNSFootp[\([2]\)]\ /. \ {x2[t] \[Rule] 0, x3[t] \[Rule] 0}]\)], "Input"], Cell[BoxData[ \(\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2] + \((w\ Cos[x5[t]]\ Sin[x4[t]] + 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2])\)\ Tan[gamma]\)], "Output"] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[{ \(\(Avect\ = \ {{D[height, x4[t]], D[height, x5[t]], D[height, x6[t]], D[height, x7[t]]}};\)\), "\[IndentingNewLine]", \(holonomicTraj\ = \ First[First[ Avect . {{\(x4'\)[t]}, {\(x5'\)[t]}, {\(x6'\)[t]}, {\(x7'\)[ t]}}]]\ // FullSimplify\)}], "Input"], Cell[BoxData[ RowBox[{ RowBox[{\((w\ Cos[x4[t]]\ Cos[x5[t]] + 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2])\), " ", \(Tan[gamma]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\((Sin[ x5[t]]\ \((l\ Cos[x6[t]] - l\ Cos[x6[t] + x7[t]] + w\ Sin[x4[t]]\ Tan[gamma])\) + Cos[x5[t]]\ \((w - 2\ l\ Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[x7[t]\/2]\ Tan[ gamma])\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "l", " ", \(Sin[x7[t]\/2]\), " ", \((Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2] + \((Cos[x6[t] + x7[t]\/2]\ Sin[x4[t]]\ Sin[ x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Tan[ gamma])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "l", " ", \((Cos[x4[t]]\ Cos[x6[t] + x7[t]]\ Tan[gamma] + Sin[x6[t] + x7[t]]\ \((Cos[x5[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Tan[gamma])\))\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]], "Output"] }, Open ]], Cell["The guard is the zero-level set of the height function:", "Text"], Cell[CellGroupData[{ Cell[BoxData[ \(\(\(height\)\(\ \)\(\[Equal]\)\(\ \)\(0\)\(\ \)\)\)], "Input"], Cell[BoxData[ \(\(-w\)\ Sin[x5[t]] + 2\ l\ Cos[x5[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2] + \((w\ Cos[x5[t]]\ Sin[x4[t]] + 2\ l\ \((Cos[x4[t]]\ Cos[x6[t] + x7[t]\/2] + Sin[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2])\)\ Tan[gamma] \[Equal] 0\)], "Output"] }, Open ]], Cell["\<\ and the negative region of the holonomic constraint's trajectory:\ \>", "Text"], Cell[CellGroupData[{ Cell[BoxData[ \(holonomicTraj\ < \ 0\)], "Input"], Cell[BoxData[ RowBox[{ RowBox[{ RowBox[{\((w\ Cos[x4[t]]\ Cos[x5[t]] + 2\ l\ \((\(-Cos[x6[t] + x7[t]\/2]\)\ Sin[x4[t]] + Cos[x4[t]]\ Sin[x5[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Sin[ x7[t]\/2])\), " ", \(Tan[gamma]\), " ", RowBox[{ SuperscriptBox["x4", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{\((Sin[ x5[t]]\ \((l\ Cos[x6[t]] - l\ Cos[x6[t] + x7[t]] + w\ Sin[x4[t]]\ Tan[gamma])\) + Cos[x5[t]]\ \((w - 2\ l\ Sin[x4[t]]\ Sin[x6[t] + x7[t]\/2]\ Sin[ x7[t]\/2]\ Tan[gamma])\))\), " ", RowBox[{ SuperscriptBox["x5", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "2", " ", "l", " ", \(Sin[x7[t]\/2]\), " ", \((Cos[x5[t]]\ Cos[ x6[t] + x7[t]\/2] + \((Cos[x6[t] + x7[t]\/2]\ Sin[x4[t]]\ Sin[ x5[t]] - Cos[x4[t]]\ Sin[x6[t] + x7[t]\/2])\)\ Tan[ gamma])\), " ", RowBox[{ SuperscriptBox["x6", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ "l", " ", \((Cos[x4[t]]\ Cos[x6[t] + x7[t]]\ Tan[gamma] + Sin[x6[t] + x7[t]]\ \((Cos[x5[t]] + Sin[x4[t]]\ Sin[x5[t]]\ Tan[gamma])\))\), " ", RowBox[{ SuperscriptBox["x7", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "<", "0"}]], "Output"] }, Open ]] }, Open ]] }, Open ]] }, FrontEndVersion->"5.1 for Microsoft Windows", ScreenRectangle->{{0, 1600}, {0, 975}}, WindowSize->{1592, 461}, WindowMargins->{{0, Automatic}, {0, Automatic}} ] (******************************************************************* Cached data follows. If you edit this Notebook file directly, not using Mathematica, you must remove the line containing CacheID at the top of the file. The cache data will then be recreated when you save this file from within Mathematica. *******************************************************************) (*CellTagsOutline CellTagsIndex->{} *) (*CellTagsIndex CellTagsIndex->{} *) (*NotebookFileOutline Notebook[{ Cell[CellGroupData[{ Cell[1776, 53, 100, 2, 95, "Title"], Cell[1879, 57, 211, 6, 110, "Subsubtitle"], Cell[2093, 65, 107, 2, 50, "Input"], Cell[CellGroupData[{ Cell[2225, 71, 28, 0, 73, "Section"], Cell[2256, 73, 239, 4, 90, "Input"], Cell[CellGroupData[{ Cell[2520, 81, 31, 0, 38, "Subsection"], Cell[2554, 83, 256, 5, 110, "Input"] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell[2859, 94, 50, 0, 73, "Section"], Cell[CellGroupData[{ Cell[2934, 98, 36, 0, 38, "Subsection"], Cell[2973, 100, 183, 3, 50, "Input"], Cell[3159, 105, 529, 9, 150, "Input"], Cell[3691, 116, 367, 6, 70, "Input"], Cell[CellGroupData[{ Cell[4083, 126, 837, 15, 130, "Input"], Cell[4923, 143, 1303, 27, 137, "Output"], Cell[6229, 172, 1447, 29, 137, "Output"], Cell[7679, 203, 1906, 37, 137, "Output"] }, Open ]], Cell[9600, 243, 638, 12, 70, "Input"], Cell[CellGroupData[{ Cell[10263, 259, 198, 4, 30, "Input"], Cell[10464, 265, 8932, 149, 201, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[19433, 419, 87, 1, 30, "Input"], Cell[19523, 422, 6570, 112, 201, "Output"] }, Open ]], Cell[26108, 537, 130, 3, 30, "Input"], Cell[CellGroupData[{ Cell[26263, 544, 198, 3, 30, "Input"], Cell[26464, 549, 4782, 101, 138, "Output"] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell[31295, 656, 36, 0, 38, "Subsection"], Cell[CellGroupData[{ Cell[31356, 660, 526, 7, 150, "Input"], Cell[31885, 669, 412, 12, 87, "Output"], Cell[32300, 683, 412, 12, 87, "Output"], Cell[32715, 697, 412, 12, 87, "Output"], Cell[33130, 711, 451, 12, 87, "Output"], Cell[33584, 725, 451, 12, 87, "Output"], Cell[34038, 739, 451, 12, 87, "Output"], Cell[34492, 753, 521, 14, 87, "Output"] }, Open ]], Cell[35028, 770, 210, 3, 70, "Input"], Cell[CellGroupData[{ Cell[35263, 777, 376, 9, 70, "Input"], Cell[35642, 788, 598, 14, 113, "Output"], Cell[36243, 804, 707, 17, 113, "Output"], Cell[36953, 823, 353, 5, 22, "Message"], Cell[37309, 830, 1085, 23, 113, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[38431, 858, 137, 3, 30, "Input"], Cell[38571, 863, 228, 4, 42, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[38836, 872, 49, 1, 30, "Input"], Cell[38888, 875, 195, 4, 42, "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell[39144, 886, 37, 0, 73, "Section"], Cell[39184, 888, 228, 4, 30, "Input"], Cell[CellGroupData[{ Cell[39437, 896, 545, 12, 90, "Input"], Cell[39985, 910, 14019, 302, 290, "Output"], Cell[54007, 1214, 8896, 180, 195, "Output"], Cell[62906, 1396, 5945, 122, 138, "Output"], Cell[68854, 1520, 3907, 84, 100, "Output"] }, Open ]], Cell[72776, 1607, 193, 3, 30, "Input"], Cell[CellGroupData[{ Cell[72994, 1614, 36, 0, 38, "Subsection"], Cell[CellGroupData[{ Cell[73055, 1618, 283, 4, 30, "Input"], Cell[73341, 1624, 19627, 359, 393, "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell[93029, 1990, 35, 0, 73, "Section"], Cell[CellGroupData[{ Cell[93089, 1994, 220, 5, 50, "Input"], Cell[93312, 2001, 363, 5, 22, "Message"], Cell[93678, 2008, 876, 20, 117, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[94591, 2033, 783, 12, 130, "Input"], Cell[95377, 2047, 1985, 38, 101, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[97399, 2090, 135, 3, 30, "Input"], Cell[97537, 2095, 2123, 42, 213, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[99697, 2142, 149, 4, 30, "Input"], Cell[99849, 2148, 12512, 211, 281, "Output"] }, Open ]], Cell[112376, 2362, 91, 1, 30, "Input"], Cell[CellGroupData[{ Cell[112492, 2367, 187, 5, 30, "Input"], Cell[112682, 2374, 12833, 255, 245, "Output"] }, Open ]], Cell[125530, 2632, 65, 0, 33, "Text"], Cell[125598, 2634, 175, 3, 30, "Input"], Cell[125776, 2639, 139, 3, 33, "Text"], Cell[125918, 2644, 75, 1, 30, "Input"], Cell[CellGroupData[{ Cell[126018, 2649, 289, 4, 90, "Input"], Cell[126310, 2655, 42, 1, 29, "Output"], Cell[126355, 2658, 42, 1, 29, "Output"], Cell[126400, 2661, 42, 1, 29, "Output"], Cell[126445, 2664, 42, 1, 29, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[126524, 2670, 33, 0, 38, "Subsection"], Cell[126560, 2672, 317, 7, 70, "Input"], Cell[CellGroupData[{ Cell[126902, 2683, 289, 4, 90, "Input"], Cell[127194, 2689, 801, 19, 53, "Output"], Cell[127998, 2710, 805, 19, 53, "Output"], Cell[128806, 2731, 2806, 55, 146, "Output"], Cell[131615, 2788, 871, 21, 54, "Output"] }, Open ]] }, Open ]] }, Open ]], Cell[CellGroupData[{ Cell[132547, 2816, 34, 0, 73, "Section"], Cell[132584, 2818, 46, 0, 33, "Text"], Cell[CellGroupData[{ Cell[132655, 2822, 180, 4, 30, "Input"], Cell[132838, 2828, 333, 6, 42, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[133208, 2839, 330, 6, 50, "Input"], Cell[133541, 2847, 1551, 34, 130, "Output"] }, Open ]], Cell[135107, 2884, 71, 0, 33, "Text"], Cell[CellGroupData[{ Cell[135203, 2888, 82, 1, 30, "Input"], Cell[135288, 2891, 354, 6, 42, "Output"] }, Open ]], Cell[135657, 2900, 89, 2, 33, "Text"], Cell[CellGroupData[{ Cell[135771, 2906, 54, 1, 30, "Input"], Cell[135828, 2909, 1651, 36, 130, "Output"] }, Open ]] }, Open ]] }, Open ]] } ] *) (******************************************************************* End of Mathematica Notebook file. *******************************************************************)