```python
from IPython.core.display import display_html
from urllib.request import urlopen

link = 'https://gist.github.com/robblack007/eca03fa9f7586860235d/raw/ef05a2f29febc94a9c9f99ca20fd0b65e74ed451/custom.css'
display_html(urlopen(link).read(), raw=True)
```


<link href='http://fonts.googleapis.com/css?family=EB+Garamond' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Lato:100,100italic' rel='stylesheet' type='text/css'>
<link rel="stylesheet" href="//cdn.jsdelivr.net/font-hack/2.013/css/hack-extended.min.css">

<style>
	@font-face{
   		font-family: "Computer Modern";
        	src: url('http://mirrors.ctan.org/fonts/cm-unicode/fonts/otf/cmunss.otf');}

        div.cell{
	        max-width:    800px;
	        margin-left:  auto;
	        margin-right: auto;}

	h1 {
        	font-family: 'Lato', sans-serif;}

	h2 {
        	font-family: 'Lato', serif;}

	h3{
		font-family:   'Lato', serif;
        	margin-top:    12px;
		margin-bottom: 3px;}

        h4{
		font-family: 'Lato', serif;}

	h5 {
        	font-family: 'Lato', serif;}

	div.text_cell_render{
        	font-family:  'EB Garamond', 'Computer Modern', 'Helvetica Neue', Arial, Helvetica, Geneva, sans-serif;
	        line-height:  140%;
	        font-size:    14pt;
		max-width:    650px;
		margin-left:  auto;
        	margin-right: auto;}

	.CodeMirror{
        	font-family: 'Inconsolata', monospace;}

	.text_cell_render h1 {
	        font-weight:   100;
	        font-size:     48pt;
		line-height:   100%;
	        color:         #CD2305;
	        margin-bottom: 0.5em;
	        margin-top:    0.5em;
	        display:       block;}

	.text_cell_render h2 {
        	font-weight:   100;
        	font-size:     36pt;}

	.text_cell_render h3 {
        	font-weight:   100;
        	font-size:     21pt;}

	.text_cell_render h4 {
        	font-weight:   100;
        	font-size:     18pt;}

	.text_cell_render h5 {
	        font-weight:   100;
	        font-size:     16pt;
	        color:         #CD2305;
	        font-style:    italic;
	        margin-bottom: 0.5em;
	        margin-top:    0.5em;
	        display:       block;}

	.text_cell_render h6 {
	        font-weight:   100;
	        font-size:     14pt;
	        color:         #CD2305;
	        font-style:    italic;
	        margin-bottom: 0.5em;
	        margin-top:    0.5em;
	        display:       block;}

	.warning{
	    	background-color: #FFFFE0;
	    	border-color: #FFCC99;
	    	border-left: 5px solid #FFCC99;
	    	padding: 0.5em;}

	.error{
	    	background-color: #fcf2f2;
	    	border-color: #dFb5b4;
	    	border-left: 5px solid #dfb5b4;
	    	padding: 0.5em;}

</style>




# Dinámica de un robot manipulador planar


```python
from sympy import var, sin, cos, Matrix, Integer, eye, Function, Rational, exp, Symbol, I, solve, pi, trigsimp, dsolve, sinh, cosh, simplify
from sympy.physics.mechanics import mechanics_printing
mechanics_printing()
```


```python
var("m1 m2 J1 J2 L1 L2 l1 l2 t g")
```


```python
q1 = Function("q1")(t)
q2 = Function("q2")(t)
```


```python
x1 = l1*cos(q1)
y1 = l1*sin(q1)
v1 = x1.diff("t")**2 + y1.diff("t")**2
v1.trigsimp()
```


```python
x2 = L1*cos(q1) + l2*cos(q1 + q2)
y2 = L1*sin(q1) + l2*sin(q1 + q2)
v2 = x2.diff("t")**2 + y2.diff("t")**2
v2.trigsimp()
```


```python
ω1 = q1.diff("t")
ω2 = q2.diff("t")
```


```python
K1 = Rational(1, 2)*m1*v1 + Rational(1, 2)*J1*ω1**2
K1
```


```python
K2 = Rational(1, 2)*m1*v2 + Rational(1, 2)*J2*ω2**2
K2
```


```python
U1 = m1*g*y1
U1
```


```python
U2 = m2*g*y2
U2
```


```python
K = K1 + K2
K
```


```python
U = U1 + U2
U
```


```python
L = (K - U).expand().simplify()
L
```


```python
τ1 = (L.diff(q1.diff(t)).diff(t) - L.diff(q1)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
```


```python
τ2 = (L.diff(q2.diff(t)).diff(t) - L.diff(q2)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
```


```python
τ1
```


```python
τ2
```


```python
from scipy.integrate import odeint
from numpy import linspace
```


```python
def pendulo_doble(estado, tiempo):
    # Se importan funciones necesarias
    from numpy import sin, cos, matrix
    # Se desenvuelven variables del estado y tiempo
    q1, q2, q̇1, q̇2 = estado
    t = tiempo
    
    # Se declaran constantes del sistema
    m1, m2 = 1, 1
    l1, l2 = 1, 1
    g = 9.81
    
    # Se declaran constantes del control
    kp1, kp2 = -30, -60
    kv1, kv2 = -20, -20
    
    # Señales de control nulas
    tau1, tau2 = 0, 0
    
    # Posiciones a alcanzar
    qd1, qd2 = 1, 1
    
    # Se declaran señales de control del sistema
    #tau1 = kp1*(q1 - qd1) + kv1*q̇1
    #tau2 = kp2*(q2 - qd2) + kv2*q̇2
    
    # Se calculan algunos terminos comunes
    ϕ1 = m1*l1**2
    ϕ2 = m1*l1*l2
    ϕ3 = m1*l2**2
    
    # Se calculan las matrices de masas, Coriolis,
    # y vectores de gravedad, control, posicion y velocidad
    M = matrix([[2*ϕ1 + 2*ϕ2*cos(q2) + ϕ3, ϕ2*cos(q2) + ϕ3],
                [ϕ2*cos(q2) + ϕ3, ϕ3]])
    C = matrix([[-2*ϕ2*sin(q2)*q̇2, -ϕ2*sin(q2)*q̇2], [ϕ2*sin(q2)*q̇1, 0]])
    G = matrix([[m1*l1*cos(q1) + m2*l1*cos(q1) + m2*l2*cos(q1 + q2)], [m2*l2*cos(q1 + q2)]])
    Tau = matrix([[tau1], [tau2]])
    q = matrix([[q1], [q2]])
    q̇ = matrix([[q̇1], [q̇2]])
    
    # Se calcula la derivada del estado del sistema
    qp1 = q̇1
    qp2 = q̇2
    
    qpp = M.I*(Tau - C*q̇ - G)
    qpp1, qpp2 = qpp.tolist()
    
    return [qp1, qp2, qpp1[0], qpp2[0]]
```


```python
t = linspace(0, 10, 1000)
estados_simulados = odeint(func = pendulo_doble, y0  = [0, 0, 0, 0], t = t)
```


```python
q1, q2, q̇1, q̇2 = list(zip(*estados_simulados.tolist()))
```


```python
%matplotlib notebook
from matplotlib.pyplot import plot, style, figure
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")
```


```python
fig1 = figure(figsize=(8, 8))

ax1 = fig1.gca()

p1, = ax1.plot(t, q1)
p2, = ax1.plot(t, q2)
ax1.legend([p1, p2],[r"$q_1$", r"$q_2$"])
ax1.set_ylim(-4, 4)
ax1.set_xlim(-0.1, 10);
```


    <IPython.core.display.Javascript object>







```python

```

ipynb
c42933b65802353ad7b3e16641cbf2e9b75198ad
Jupyter Notebook
f38cb358f2681e9c0dce979acbdcd81bf63bd59c
MIT
robblack007/clase-dinamica-robot
Clases/.ipynb_checkpoints/Dinamica pendulo doble completa-checkpoint.ipynb
f38cb358f2681e9c0dce979acbdcd81bf63bd59c
2016-05-30T23:58:07.000Z
2016-01-26T18:33:11.000Z
MIT
robblack007/clase-dinamica-robot
Clases/.ipynb_checkpoints/Dinamica pendulo doble completa-checkpoint.ipynb
f38cb358f2681e9c0dce979acbdcd81bf63bd59c
MIT
robblack007/clase-dinamica-robot
Clases/.ipynb_checkpoints/Dinamica pendulo doble completa-checkpoint.ipynb