{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Introduction" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We now have defined all of the necessary components to derive the equations of motion of the system using Kane's Method:\n", "\n", "- Linear velocity of each mass center.\n", "- Angular velocity of each rigid body.\n", "- All contributing (external) forces.\n", "\n", "We will use the `KanesMethod` class which provides an automated computation of the first order ordinary differential equations given the above quantities. (Any method can be used, such as Newton-Euler or Hamilton's method, but there are only automated methods for Kane's equations and Lagrangian dynamics at the moment)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Setup" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Import the solutions from the previous notebooks:" ] }, { "cell_type": "code", "execution_count": 1, "metadata": { "collapsed": false }, "outputs": [], "source": [ "from __future__ import print_function, division\n", "from solution.kinetics import *" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will need the `KanesMethod` class and `trigsimp` to give a reasonably compact result." ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "collapsed": false }, "outputs": [], "source": [ "from sympy import trigsimp\n", "from sympy.physics.mechanics import KanesMethod" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Enable nice math printing:" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "collapsed": false }, "outputs": [], "source": [ "from sympy.physics.vector import init_vprinting\n", "init_vprinting(use_latex='mathjax')" ] }, { "cell_type": "code", "execution_count": 4, "metadata": { "collapsed": false }, "outputs": [], "source": [ "KanesMethod?" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Equations of Motion" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "At the bare minimum for unconstrained systems, the `KanesMethod` class needs to know the generalized coordinates, the generalized speeds, the kinematical differential equations, the loads, the bodies, and a Newtonian reference frame. First, make a list of the generalized coordinates, i.e. the three joint angles:" ] }, { "cell_type": "code", "execution_count": 5, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left [ \\theta_{1}, \\quad \\theta_{2}, \\quad \\theta_{3}\\right ]$$" ], "text/plain": [ "[θ₁, θ₂, θ₃]" ] }, "execution_count": 5, "metadata": {}, "output_type": "execute_result" } ], "source": [ "coordinates = [theta1, theta2, theta3]\n", "coordinates" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now list the generalized speeds, i.e. the joint rates:" ] }, { "cell_type": "code", "execution_count": 6, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left [ \\omega_{1}, \\quad \\omega_{2}, \\quad \\omega_{3}\\right ]$$" ], "text/plain": [ "[ω₁, ω₂, ω₃]" ] }, "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ "speeds = [omega1, omega2, omega3]\n", "speeds" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The `KanesMethod` object can now be created with the inertial reference frame, coordinates, speeds, and kinematical differential equations:" ] }, { "cell_type": "code", "execution_count": 7, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left [ \\omega_{1} - \\dot{\\theta}_{1}, \\quad \\omega_{2} - \\dot{\\theta}_{2}, \\quad \\omega_{3} - \\dot{\\theta}_{3}\\right ]$$" ], "text/plain": [ "[ω₁ - θ₁̇, ω₂ - θ₂̇, ω₃ - θ₃̇]" ] }, "execution_count": 7, "metadata": {}, "output_type": "execute_result" } ], "source": [ "kinematical_differential_equations" ] }, { "cell_type": "code", "execution_count": 8, "metadata": { "collapsed": false }, "outputs": [], "source": [ "kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now list the six loads we defined and the three bodies:" ] }, { "cell_type": "code", "execution_count": 9, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/plain": [ "[(L_o, - g*m_L*I.y),\n", " (U_o, - g*m_U*I.y),\n", " (T_o, - g*m_T*I.y),\n", " (L, (T_a - T_k)*I.z),\n", " (U, (-T_h + T_k)*I.z),\n", " (T, T_h*I.z)]" ] }, "execution_count": 9, "metadata": {}, "output_type": "execute_result" } ], "source": [ "loads = [lower_leg_grav_force,\n", " upper_leg_grav_force,\n", " torso_grav_force, \n", " lower_leg_torque,\n", " upper_leg_torque,\n", " torso_torque]\n", "loads" ] }, { "cell_type": "code", "execution_count": 10, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/plain": [ "[Lower Leg, Upper Leg, Torso]" ] }, "execution_count": 10, "metadata": {}, "output_type": "execute_result" } ], "source": [ "bodies = [lower_leg, upper_leg, torso]\n", "bodies" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The equations of motion can now be computed using the `kanes_equations` method, which takes the list of loads and bodies. It returns the equations of motion (i.e. first order ordinary differential equations) in Kane's form:\n", "\n", "$$ F_r + F_r^* = 0$$\n", "\n", "which is essentially equivalent to the classic Newton_Euler form:\n", "\n", "$$ \\mathbf{F} = \\mathbf{m}\\mathbf{a} $$\n", "\n", "$$ \\mathbf{T} = \\mathbf{I} \\mathbf{\\alpha} $$" ] }, { "cell_type": "code", "execution_count": 11, "metadata": { "collapsed": false }, "outputs": [], "source": [ "fr, frstar = kane.kanes_equations(loads, bodies)" ] }, { "cell_type": "code", "execution_count": 12, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left[\\begin{matrix}d_{L} g m_{L} \\operatorname{sin}\\left(\\theta_{1}\\right) + d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) + d_{T} l_{L} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{U} g m_{U} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) + d_{U} l_{L} m_{U} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2}\\right) - d_{U} l_{L} m_{U} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + g l_{L} m_{T} \\operatorname{sin}\\left(\\theta_{1}\\right) + g l_{L} m_{U} \\operatorname{sin}\\left(\\theta_{1}\\right) + g l_{U} m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) + l_{L} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2}\\right) - l_{L} l_{U} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) - \\left(I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\right) \\dot{\\omega}_{3} - \\left(I_{Tz} + I_{Uz} + d_{U} m_{U} \\left(d_{U} + l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right)\\right) + m_{T} \\left(d_{T}^{2} + d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right)\\right) \\dot{\\omega}_{2} - \\left(I_{Lz} + I_{Tz} + I_{Uz} + d_{L}^{2} m_{L} + m_{T} \\left(d_{T}^{2} + 2 d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L}^{2} + 2 l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right) + m_{U} \\left(d_{U}^{2} + 2 d_{U} l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{L}^{2}\\right)\\right) \\dot{\\omega}_{1} + T_{a}\\\\d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{U} g m_{U} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) - d_{U} l_{L} m_{U} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + g l_{U} m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) - l_{L} l_{U} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) - \\left(I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\right) \\dot{\\omega}_{3} - \\left(I_{Tz} + I_{Uz} + d_{U}^{2} m_{U} + m_{T} \\left(d_{T}^{2} + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{U}^{2}\\right)\\right) \\dot{\\omega}_{2} - \\left(I_{Tz} + I_{Uz} + d_{U} m_{U} \\left(d_{U} + l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right)\\right) + m_{T} \\left(d_{T}^{2} + d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right)\\right) \\dot{\\omega}_{1} + T_{k}\\\\d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) - \\left(I_{Tz} + d_{T}^{2} m_{T}\\right) \\dot{\\omega}_{3} - \\left(I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\right) \\dot{\\omega}_{2} - \\left(I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\right) \\dot{\\omega}_{1} + T_{h}\\end{matrix}\\right]$$" ], "text/plain": [ "⎡ 2\n", "⎢d_L⋅g⋅m_L⋅sin(θ₁) + d_T⋅g⋅m_T⋅sin(θ₁ + θ₂ + θ₃) + d_T⋅l_L⋅m_T⋅(ω₁ + ω₂ + ω₃) \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎣ \n", "\n", " 2 2 \n", "⋅sin(θ₂ + θ₃) - d_T⋅l_L⋅m_T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₃) \n", " \n", " \n", " d_T⋅g⋅m_T⋅si\n", " \n", " \n", " \n", "\n", " 2 \n", "+ d_T⋅l_U⋅m_T⋅(ω₁ + ω₂ + ω₃) ⋅sin(θ₃) + d_U⋅g⋅m_U⋅sin(θ₁ + θ₂) + d_U⋅l_L⋅m_U⋅(\n", " \n", " 2 2 \n", "n(θ₁ + θ₂ + θ₃) - d_T⋅l_L⋅m_T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₃\n", " \n", " \n", " \n", "\n", " 2 2 \n", "ω₁ + ω₂) ⋅sin(θ₂) - d_U⋅l_L⋅m_U⋅ω₁ ⋅sin(θ₂) + g⋅l_L⋅m_T⋅sin(θ₁) + g⋅l_L⋅m_U⋅si\n", " \n", " 2 \n", ") + d_T⋅l_U⋅m_T⋅(ω₁ + ω₂ + ω₃) ⋅sin(θ₃) + d_U⋅g⋅m_U⋅sin(θ₁ + θ₂) - d_U⋅l_L⋅m_U\n", " \n", " \n", " d_T⋅g⋅m_T⋅sin(θ₁ + θ₂ + θ₃) - d_T⋅l_L⋅m_\n", "\n", " 2 \n", "n(θ₁) + g⋅l_U⋅m_T⋅sin(θ₁ + θ₂) + l_L⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₂) - l_L⋅l_U⋅m_T⋅\n", " \n", " 2 2 \n", "⋅ω₁ ⋅sin(θ₂) + g⋅l_U⋅m_T⋅sin(θ₁ + θ₂) - l_L⋅l_U⋅m_T⋅ω₁ ⋅sin(θ₂) - (I_Tz + d_T⋅\n", " \n", " 2 2 ⎛ 2 ⎞ \n", "T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₃) - ⎝I_Tz + d_T ⋅m_T⎠⋅ω₃̇ - \n", "\n", " 2 ⎛ \n", "ω₁ ⋅sin(θ₂) - (I_Tz + d_T⋅m_T⋅(d_T + l_L⋅cos(θ₂ + θ₃) + l_U⋅cos(θ₃)))⋅ω₃̇ - ⎝I\n", " \n", " ⎛ 2 ⎛ 2 \n", "m_T⋅(d_T + l_U⋅cos(θ₃)))⋅ω₃̇ - ⎝I_Tz + I_Uz + d_U ⋅m_U + m_T⋅⎝d_T + 2⋅d_T⋅l_U\n", " \n", " \n", "(I_Tz + d_T⋅m_T⋅(d_T + l_U⋅cos(θ₃)))⋅ω₂̇ - (I_Tz + d_T⋅m_T⋅(d_T + l_L⋅cos(θ₂ +\n", "\n", " ⎛ 2 \n", "_Tz + I_Uz + d_U⋅m_U⋅(d_U + l_L⋅cos(θ₂)) + m_T⋅⎝d_T + d_T⋅l_L⋅cos(θ₂ + θ₃) + \n", " \n", " 2⎞⎞ ⎛ ⎛ 2\n", "⋅cos(θ₃) + l_U ⎠⎠⋅ω₂̇ - ⎝I_Tz + I_Uz + d_U⋅m_U⋅(d_U + l_L⋅cos(θ₂)) + m_T⋅⎝d_T \n", " \n", " \n", " θ₃) + l_U⋅cos(θ₃)))⋅ω₁̇ + T_h \n", "\n", " 2⎞⎞ ⎛ 2 \n", "2⋅d_T⋅l_U⋅cos(θ₃) + l_L⋅l_U⋅cos(θ₂) + l_U ⎠⎠⋅ω₂̇ - ⎝I_Lz + I_Tz + I_Uz + d_L ⋅\n", " \n", " 2⎞⎞ \n", " + d_T⋅l_L⋅cos(θ₂ + θ₃) + 2⋅d_T⋅l_U⋅cos(θ₃) + l_L⋅l_U⋅cos(θ₂) + l_U ⎠⎠⋅ω₁̇ + T\n", " \n", " \n", " \n", "\n", " ⎛ 2 2 \n", "m_L + m_T⋅⎝d_T + 2⋅d_T⋅l_L⋅cos(θ₂ + θ₃) + 2⋅d_T⋅l_U⋅cos(θ₃) + l_L + 2⋅l_L⋅l_\n", " \n", " \n", "_k \n", " \n", " \n", " \n", "\n", " 2⎞ ⎛ 2 2⎞⎞ ⎤\n", "U⋅cos(θ₂) + l_U ⎠ + m_U⋅⎝d_U + 2⋅d_U⋅l_L⋅cos(θ₂) + l_L ⎠⎠⋅ω₁̇ + Tₐ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎦" ] }, "execution_count": 12, "metadata": {}, "output_type": "execute_result" } ], "source": [ "trigsimp(fr + frstar)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Keep in mind that out utlimate goal is to have the equations of motion in first order form:\n", "\n", "$$ \\dot{\\mathbf{x}} = \\mathbf{g}(\\mathbf{x}, t) $$\n", "\n", "The equations of motion are linear in terms of the derivatives of the generalized speeds and the `KanesMethod` class automatically puts the equations in a more useful form for the next step of numerical simulation:\n", "\n", "$$ \\mathbf{M}(\\mathbf{x}, t)\\dot{\\mathbf{x}} = \\mathbf{f}(\\mathbf{x}, t) $$\n", "\n", "Note that\n", "\n", "$$ \\mathbf{g} = \\mathbf{M}^{-1}(\\mathbf{x}, t) \\mathbf{f}(\\mathbf{x}, t) $$\n", "\n", "and that $\\mathbf{g}$ can be computed analytically but for non-toy problems, it is best to do this numerically. So we will simply generate the $\\mathbf{M}$ and $\\mathbf{f}$ matrices for later use.\n", "\n", "The mass matrix, $\\mathbf{M}$, can be accessed with the `mass_matrix` method (use `mass_matrix_full` to include the kinematical differential equations too. We can use `trigsimp` again to make this relatively compact: " ] }, { "cell_type": "code", "execution_count": 13, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left[\\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\\\0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & I_{Lz} + I_{Tz} + I_{Uz} + d_{L}^{2} m_{L} + m_{T} \\left(d_{T}^{2} + 2 d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L}^{2} + 2 l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right) + m_{U} \\left(d_{U}^{2} + 2 d_{U} l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{L}^{2}\\right) & I_{Tz} + I_{Uz} + d_{U} m_{U} \\left(d_{U} + l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right)\\right) + m_{T} \\left(d_{T}^{2} + d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right) & I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\\\0 & 0 & 0 & I_{Tz} + I_{Uz} + d_{U} m_{U} \\left(d_{U} + l_{L} \\operatorname{cos}\\left(\\theta_{2}\\right)\\right) + m_{T} \\left(d_{T}^{2} + d_{T} l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{L} l_{U} \\operatorname{cos}\\left(\\theta_{2}\\right) + l_{U}^{2}\\right) & I_{Tz} + I_{Uz} + d_{U}^{2} m_{U} + m_{T} \\left(d_{T}^{2} + 2 d_{T} l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right) + l_{U}^{2}\\right) & I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right)\\\\0 & 0 & 0 & I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{L} \\operatorname{cos}\\left(\\theta_{2} + \\theta_{3}\\right) + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right) & I_{Tz} + d_{T} m_{T} \\left(d_{T} + l_{U} \\operatorname{cos}\\left(\\theta_{3}\\right)\\right) & I_{Tz} + d_{T}^{2} m_{T}\\end{matrix}\\right]$$" ], "text/plain": [ "⎡1 0 0 \n", "⎢ \n", "⎢0 1 0 \n", "⎢ \n", "⎢0 0 1 \n", "⎢ \n", "⎢ 2 ⎛ 2 \n", "⎢0 0 0 I_Lz + I_Tz + I_Uz + d_L ⋅m_L + m_T⋅⎝d_T + 2⋅d_T⋅l_L⋅cos(θ₂ + θ₃) +\n", "⎢ \n", "⎢ ⎛\n", "⎢0 0 0 I_Tz + I_Uz + d_U⋅m_U⋅(d_U + l_L⋅cos(θ₂)) + m_T⋅⎝\n", "⎢ \n", "⎢ \n", "⎣0 0 0 I_Tz + d_T⋅m_T\n", "\n", " 0 \n", " \n", " 0 \n", " \n", " 0 \n", " \n", " 2 2⎞ ⎛ 2 \n", " 2⋅d_T⋅l_U⋅cos(θ₃) + l_L + 2⋅l_L⋅l_U⋅cos(θ₂) + l_U ⎠ + m_U⋅⎝d_U + 2⋅d_U⋅l_L⋅\n", " \n", " 2 2⎞ \n", "d_T + d_T⋅l_L⋅cos(θ₂ + θ₃) + 2⋅d_T⋅l_U⋅cos(θ₃) + l_L⋅l_U⋅cos(θ₂) + l_U ⎠ \n", " \n", " \n", "⋅(d_T + l_L⋅cos(θ₂ + θ₃) + l_U⋅cos(θ₃)) \n", "\n", " 0\n", " \n", " 0\n", " \n", " 0\n", " \n", " 2⎞ ⎛ 2 \n", "cos(θ₂) + l_L ⎠ I_Tz + I_Uz + d_U⋅m_U⋅(d_U + l_L⋅cos(θ₂)) + m_T⋅⎝d_T + d_T⋅l\n", " \n", " 2 ⎛ \n", " I_Tz + I_Uz + d_U ⋅m_U + m_T⋅⎝d\n", " \n", " \n", " I_Tz + d_T⋅m_T⋅(d\n", "\n", " \n", " \n", " \n", " \n", " \n", " \n", " 2⎞ \n", "_L⋅cos(θ₂ + θ₃) + 2⋅d_T⋅l_U⋅cos(θ₃) + l_L⋅l_U⋅cos(θ₂) + l_U ⎠ I_Tz + d_T⋅m_T⋅\n", " \n", " 2 2⎞ \n", "_T + 2⋅d_T⋅l_U⋅cos(θ₃) + l_U ⎠ I_Tz +\n", " \n", " \n", "_T + l_U⋅cos(θ₃)) \n", "\n", " 0 ⎤\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " ⎥\n", "(d_T + l_L⋅cos(θ₂ + θ₃) + l_U⋅cos(θ₃))⎥\n", " ⎥\n", " ⎥\n", " d_T⋅m_T⋅(d_T + l_U⋅cos(θ₃)) ⎥\n", " ⎥\n", " 2 ⎥\n", " I_Tz + d_T ⋅m_T ⎦" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "mass_matrix = trigsimp(kane.mass_matrix_full)\n", "mass_matrix" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The right hand side, $\\mathbf{f}$, is a vector function of all the non-inertial forces (gyroscopic, external, coriolis, etc):" ] }, { "cell_type": "code", "execution_count": 14, "metadata": { "collapsed": false }, "outputs": [ { "data": { "text/latex": [ "$$\\left[\\begin{matrix}\\omega_{1}\\\\\\omega_{2}\\\\\\omega_{3}\\\\d_{L} g m_{L} \\operatorname{sin}\\left(\\theta_{1}\\right) + d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) + d_{T} l_{L} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{U} g m_{U} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) + d_{U} l_{L} m_{U} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2}\\right) - d_{U} l_{L} m_{U} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + g l_{L} m_{T} \\operatorname{sin}\\left(\\theta_{1}\\right) + g l_{L} m_{U} \\operatorname{sin}\\left(\\theta_{1}\\right) + g l_{U} m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) + l_{L} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{2}\\right) - l_{L} l_{U} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + T_{a}\\\\d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2} + \\omega_{3}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + d_{U} g m_{U} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) - d_{U} l_{L} m_{U} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + g l_{U} m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2}\\right) - l_{L} l_{U} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2}\\right) + T_{k}\\\\d_{T} g m_{T} \\operatorname{sin}\\left(\\theta_{1} + \\theta_{2} + \\theta_{3}\\right) - d_{T} l_{L} m_{T} \\omega^{2}_{1} \\operatorname{sin}\\left(\\theta_{2} + \\theta_{3}\\right) - d_{T} l_{U} m_{T} \\left(\\omega_{1} + \\omega_{2}\\right)^{2} \\operatorname{sin}\\left(\\theta_{3}\\right) + T_{h}\\end{matrix}\\right]$$" ], "text/plain": [ "⎡ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ 2\n", "⎢d_L⋅g⋅m_L⋅sin(θ₁) + d_T⋅g⋅m_T⋅sin(θ₁ + θ₂ + θ₃) + d_T⋅l_L⋅m_T⋅(ω₁ + ω₂ + ω₃) \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎣ \n", "\n", " \n", " \n", " \n", " \n", " \n", " \n", " 2 2 \n", "⋅sin(θ₂ + θ₃) - d_T⋅l_L⋅m_T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₃) \n", " \n", " 2 \n", " d_T⋅g⋅m_T⋅sin(θ₁ + θ₂ + θ₃) - d_T⋅l_L⋅m_T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_\n", " \n", " \n", " d\n", "\n", " ω₁ \n", " \n", " ω₂ \n", " \n", " ω₃ \n", " \n", " 2 \n", "+ d_T⋅l_U⋅m_T⋅(ω₁ + ω₂ + ω₃) ⋅sin(θ₃) + d_U⋅g⋅m_U⋅sin(θ₁ + θ₂) + d_U⋅l_L⋅m_U⋅(\n", " \n", " 2 2 \n", "T⋅(ω₁ + ω₂) ⋅sin(θ₃) + d_T⋅l_U⋅m_T⋅(ω₁ + ω₂ + ω₃) ⋅sin(θ₃) + d_U⋅g⋅m_U⋅sin(θ₁ \n", " \n", " 2 \n", "_T⋅g⋅m_T⋅sin(θ₁ + θ₂ + θ₃) - d_T⋅l_L⋅m_T⋅ω₁ ⋅sin(θ₂ + θ₃) - d_T⋅l_U⋅m_T⋅(ω₁ + \n", "\n", " \n", " \n", " \n", " \n", " \n", " \n", " 2 2 \n", "ω₁ + ω₂) ⋅sin(θ₂) - d_U⋅l_L⋅m_U⋅ω₁ ⋅sin(θ₂) + g⋅l_L⋅m_T⋅sin(θ₁) + g⋅l_L⋅m_U⋅si\n", " \n", " 2 2 \n", "+ θ₂) - d_U⋅l_L⋅m_U⋅ω₁ ⋅sin(θ₂) + g⋅l_U⋅m_T⋅sin(θ₁ + θ₂) - l_L⋅l_U⋅m_T⋅ω₁ ⋅sin\n", " \n", " 2 \n", "ω₂) ⋅sin(θ₃) + T_h \n", "\n", " \n", " \n", " \n", " \n", " \n", " \n", " 2 \n", "n(θ₁) + g⋅l_U⋅m_T⋅sin(θ₁ + θ₂) + l_L⋅l_U⋅m_T⋅(ω₁ + ω₂) ⋅sin(θ₂) - l_L⋅l_U⋅m_T⋅\n", " \n", " \n", "(θ₂) + T_k \n", " \n", " \n", " \n", "\n", " ⎤\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " 2 ⎥\n", "ω₁ ⋅sin(θ₂) + Tₐ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎦" ] }, "execution_count": 14, "metadata": {}, "output_type": "execute_result" } ], "source": [ "forcing_vector = trigsimp(kane.forcing_full)\n", "forcing_vector" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.5.1" } }, "nbformat": 4, "nbformat_minor": 0 }