{ "cells": [ { "cell_type": "code", "execution_count": 10, "metadata": {}, "outputs": [], "source": [ "'''\n", " Lagarange_equation_RR \n", " anthor : anthony Create on 2022 10 01\n", "'''\n", "import sympy\n", "import math\n", "import numpy as np\n", "sympy.init_printing(use_latex='mathjax')\n", "from IPython.display import display\n", "from sympy_tool.Robot_sympy import Robot_Sympy\n", "from sympy_tool.SympyTool import SympyTool\n", "\n", "cos = np.cos\n", "sin = np.sin\n", "pi = np.pi\n", "degTrad = np.deg2rad\n", "radTdeg = np.rad2deg\n", "eye = np.eye\n" ] }, { "cell_type": "code", "execution_count": 13, "metadata": {}, "outputs": [], "source": [ "n = 2\n", "mass = sympy.symbols(f'm0:{n+1}')\n", "I = [sympy.eye(3),sympy.eye(3)]\n", "Pc = [sympy.eye(4),sympy.eye(4)]\n", "l1 = sympy.Symbol('l1')\n", "l2 = sympy.Symbol('l2')\n", "\n", "a = sympy.Symbol('a')\n", "alpha = sympy.Symbol('α')\n", "theta = sympy.Symbol('θ')\n", "d = sympy.Symbol('d')\n", "q = sympy.symbols(f'q1:{n+1}') #input angle\n", "qd = sympy.symbols(f'qd1:{n+1}') #input angle vel\n", "tool = Robot_Sympy()\n", "\n", "test = tool.RTRT(a,alpha,d,theta)\n", "test2 = tool.RTTR(a,alpha,d,theta)\n", "\n", " \n", "# Tsrc = tool.RTTR_Matrix(pi/2,0,0,0)\n", "T1 = tool.RTTR(0,l1,0,q[0])\n", "T2 = tool.RTTR(0,l2,0,q[1])\n", "# T3 = tool.RTTR(l2,0,0,0)\n", "Ti = [] \n", "Ti.append(T1 )\n", "Ti.append(T1 @ T2 )\n", "rotate_axis = ['z','z']\n", "\n", "M,V,C,G,Jv,Jw,J,KE,Ki,PE = tool.Lagarange(n,q,qd,Ti,rotate_axis,mass,Pc,'z',None)" ] }, { "cell_type": "code", "execution_count": 14, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)} & 0 & α\\\\\\sin{\\left(θ \\right)} \\cos{\\left(a \\right)} & \\cos{\\left(a \\right)} \\cos{\\left(θ \\right)} & - \\sin{\\left(a \\right)} & - d \\sin{\\left(a \\right)}\\\\\\sin{\\left(a \\right)} \\sin{\\left(θ \\right)} & \\sin{\\left(a \\right)} \\cos{\\left(θ \\right)} & \\cos{\\left(a \\right)} & d \\cos{\\left(a \\right)}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡ cos(θ) -sin(θ) 0 α ⎤\n", "⎢ ⎥\n", "⎢sin(θ)⋅cos(a) cos(a)⋅cos(θ) -sin(a) -d⋅sin(a)⎥\n", "⎢ ⎥\n", "⎢sin(a)⋅sin(θ) sin(a)⋅cos(θ) cos(a) d⋅cos(a) ⎥\n", "⎢ ⎥\n", "⎣ 0 0 0 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)} \\cos{\\left(a \\right)} & \\sin{\\left(a \\right)} \\sin{\\left(θ \\right)} & α \\cos{\\left(θ \\right)}\\\\\\sin{\\left(θ \\right)} & \\cos{\\left(a \\right)} \\cos{\\left(θ \\right)} & - \\sin{\\left(a \\right)} \\cos{\\left(θ \\right)} & α \\sin{\\left(θ \\right)}\\\\0 & \\sin{\\left(a \\right)} & \\cos{\\left(a \\right)} & d\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡cos(θ) -sin(θ)⋅cos(a) sin(a)⋅sin(θ) α⋅cos(θ)⎤\n", "⎢ ⎥\n", "⎢sin(θ) cos(a)⋅cos(θ) -sin(a)⋅cos(θ) α⋅sin(θ)⎥\n", "⎢ ⎥\n", "⎢ 0 sin(a) cos(a) d ⎥\n", "⎢ ⎥\n", "⎣ 0 0 0 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{2} \\right)} & - \\sin{\\left(q_{2} \\right)} & 0 & l_{2} \\cos{\\left(q_{2} \\right)}\\\\\\sin{\\left(q_{2} \\right)} & \\cos{\\left(q_{2} \\right)} & 0 & l_{2} \\sin{\\left(q_{2} \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡cos(q₂) -sin(q₂) 0 l₂⋅cos(q₂)⎤\n", "⎢ ⎥\n", "⎢sin(q₂) cos(q₂) 0 l₂⋅sin(q₂)⎥\n", "⎢ ⎥\n", "⎢ 0 0 1 0 ⎥\n", "⎢ ⎥\n", "⎣ 0 0 0 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[ \\left[\\begin{matrix}\\cos{\\left(q_{1} \\right)} & - \\sin{\\left(q_{1} \\right)} & 0 & l_{1} \\cos{\\left(q_{1} \\right)}\\\\\\sin{\\left(q_{1} \\right)} & \\cos{\\left(q_{1} \\right)} & 0 & l_{1} \\sin{\\left(q_{1} \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right], \\ \\left[\\begin{matrix}- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} & - \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} & 0 & l_{1} \\cos{\\left(q_{1} \\right)} - l_{2} \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + l_{2} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\\\\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} + \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} & - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} & 0 & l_{1} \\sin{\\left(q_{1} \\right)} + l_{2} \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} + l_{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]\\right]$" ], "text/plain": [ "⎡⎡cos(q₁) -sin(q₁) 0 l₁⋅cos(q₁)⎤ ⎡-sin(q₁)⋅sin(q₂) + cos(q₁)⋅cos(q₂) -sin\n", "⎢⎢ ⎥ ⎢ \n", "⎢⎢sin(q₁) cos(q₁) 0 l₁⋅sin(q₁)⎥ ⎢sin(q₁)⋅cos(q₂) + sin(q₂)⋅cos(q₁) -sin\n", "⎢⎢ ⎥, ⎢ \n", "⎢⎢ 0 0 1 0 ⎥ ⎢ 0 \n", "⎢⎢ ⎥ ⎢ \n", "⎣⎣ 0 0 0 1 ⎦ ⎣ 0 \n", "\n", "(q₁)⋅cos(q₂) - sin(q₂)⋅cos(q₁) 0 l₁⋅cos(q₁) - l₂⋅sin(q₁)⋅sin(q₂) + l₂⋅cos(q₁\n", " \n", "(q₁)⋅sin(q₂) + cos(q₁)⋅cos(q₂) 0 l₁⋅sin(q₁) + l₂⋅sin(q₁)⋅cos(q₂) + l₂⋅sin(q₂\n", " \n", " 0 1 0 \n", " \n", " 0 0 1 \n", "\n", ")⋅cos(q₂)⎤⎤\n", " ⎥⎥\n", ")⋅cos(q₁)⎥⎥\n", " ⎥⎥\n", " ⎥⎥\n", " ⎥⎥\n", " ⎦⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\left[\\begin{matrix}- l_{1} \\sin{\\left(q_{1} \\right)} & 0\\\\l_{1} \\cos{\\left(q_{1} \\right)} & 0\\\\0 & 0\\end{matrix}\\right] & \\left[\\begin{matrix}- l_{1} \\sin{\\left(q_{1} \\right)} - l_{2} \\sin{\\left(q_{1} + q_{2} \\right)} & - l_{2} \\sin{\\left(q_{1} + q_{2} \\right)}\\\\l_{1} \\cos{\\left(q_{1} \\right)} + l_{2} \\cos{\\left(q_{1} + q_{2} \\right)} & l_{2} \\cos{\\left(q_{1} + q_{2} \\right)}\\\\0 & 0\\end{matrix}\\right]\\end{matrix}\\right]$" ], "text/plain": [ "⎡⎡-l₁⋅sin(q₁) 0⎤ ⎡-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂) -l₂⋅sin(q₁ + q₂)⎤⎤\n", "⎢⎢ ⎥ ⎢ ⎥⎥\n", "⎢⎢l₁⋅cos(q₁) 0⎥ ⎢l₁⋅cos(q₁) + l₂⋅cos(q₁ + q₂) l₂⋅cos(q₁ + q₂) ⎥⎥\n", "⎢⎢ ⎥ ⎢ ⎥⎥\n", "⎣⎣ 0 0⎦ ⎣ 0 0 ⎦⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\left[\\begin{matrix}\\left[\\begin{matrix}0\\end{matrix}\\right] & \\mathtt{\\text{0}}\\\\\\left[\\begin{matrix}0\\end{matrix}\\right] & \\mathtt{\\text{0}}\\\\\\left[\\begin{matrix}1\\end{matrix}\\right] & \\mathtt{\\text{0}}\\end{matrix}\\right] & \\left[\\begin{matrix}\\left[\\begin{matrix}0\\end{matrix}\\right] & \\left[\\begin{matrix}0\\end{matrix}\\right]\\\\\\left[\\begin{matrix}0\\end{matrix}\\right] & \\left[\\begin{matrix}0\\end{matrix}\\right]\\\\\\left[\\begin{matrix}1\\end{matrix}\\right] & \\left[\\begin{matrix}1\\end{matrix}\\right]\\end{matrix}\\right]\\end{matrix}\\right]$" ], "text/plain": [ "⎡⎡[0] 0⎤ ⎡[0] [0]⎤⎤\n", "⎢⎢ ⎥ ⎢ ⎥⎥\n", "⎢⎢[0] 0⎥ ⎢[0] [0]⎥⎥\n", "⎢⎢ ⎥ ⎢ ⎥⎥\n", "⎣⎣[1] 0⎦ ⎣[1] [1]⎦⎦" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "display(test)\n", "display(test2)\n", "\n", "display(T2)\n", "\n", "display(Ti)\n", "display(Jv)\n", "display(Jw)" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}l_{1}^{2} m_{0} + m_{1} \\left(l_{1}^{2} + 2 l_{1} l_{2} \\cos{\\left(q_{1} \\right)} + l_{2}^{2}\\right) & l_{2} m_{1} \\left(l_{1} \\cos{\\left(q_{1} \\right)} + l_{2}\\right)\\\\l_{2} m_{1} \\left(l_{1} \\cos{\\left(q_{1} \\right)} + l_{2}\\right) & l_{2}^{2} m_{1}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 ⎛ 2 2⎞ ⎤\n", "⎢l₁ ⋅m₀ + m₁⋅⎝l₁ + 2⋅l₁⋅l₂⋅cos(q₁) + l₂ ⎠ l₂⋅m₁⋅(l₁⋅cos(q₁) + l₂)⎥\n", "⎢ ⎥\n", "⎢ 2 ⎥\n", "⎣ l₂⋅m₁⋅(l₁⋅cos(q₁) + l₂) l₂ ⋅m₁ ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- 1.0 l_{1} l_{2} m_{1} qd_{1} \\sin{\\left(q_{1} \\right)} & - 1.0 l_{1} l_{2} m_{1} \\left(qd_{0} + qd_{1}\\right) \\sin{\\left(q_{1} \\right)}\\\\1.0 l_{1} l_{2} m_{1} qd_{0} \\sin{\\left(q_{1} \\right)} & 0\\end{matrix}\\right]$" ], "text/plain": [ "⎡-1.0⋅l₁⋅l₂⋅m₁⋅qd₁⋅sin(q₁) -1.0⋅l₁⋅l₂⋅m₁⋅(qd₀ + qd₁)⋅sin(q₁)⎤\n", "⎢ ⎥\n", "⎣1.0⋅l₁⋅l₂⋅m₁⋅qd₀⋅sin(q₁) 0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- 1.0 l_{1} l_{2} m_{1} qd_{0} qd_{1} \\sin{\\left(q_{1} \\right)} - 1.0 l_{1} l_{2} m_{1} qd_{1} \\left(qd_{0} + qd_{1}\\right) \\sin{\\left(q_{1} \\right)}\\\\1.0 l_{1} l_{2} m_{1} qd_{0}^{2} \\sin{\\left(q_{1} \\right)}\\end{matrix}\\right]$" ], "text/plain": [ "⎡-l₁⋅l₂⋅m₁⋅qd₀⋅qd₁⋅sin(q₁) - l₁⋅l₂⋅m₁⋅qd₁⋅(qd₀ + qd₁)⋅sin(q₁)⎤\n", "⎢ ⎥\n", "⎢ 2 ⎥\n", "⎣ 1.0⋅l₁⋅l₂⋅m₁⋅qd₀ ⋅sin(q₁) ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}g \\left(l_{1} m_{0} \\cos{\\left(q_{0} \\right)} + m_{1} \\left(l_{1} \\cos{\\left(q_{0} \\right)} + l_{2} \\cos{\\left(q_{0} + q_{1} \\right)}\\right)\\right)\\\\g l_{2} m_{1} \\cos{\\left(q_{0} + q_{1} \\right)}\\end{matrix}\\right]$" ], "text/plain": [ "⎡g⋅(l₁⋅m₀⋅cos(q₀) + m₁⋅(l₁⋅cos(q₀) + l₂⋅cos(q₀ + q₁)))⎤\n", "⎢ ⎥\n", "⎣ g⋅l₂⋅m₁⋅cos(q₀ + q₁) ⎦" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "display(M)\n", "display(C)\n", "display(V)\n", "display(G)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "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.8.10" }, "orig_nbformat": 4, "vscode": { "interpreter": { "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1" } } }, "nbformat": 4, "nbformat_minor": 2 }