1 of 25

��Equations of Motion�&�Physics Engines

Chapters 2-3, and Appendix P.4

2 of 25

Design Study A

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

2

3 of 25

Design Study B

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

3

4 of 25

Design Study C

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

4

5 of 25

Design Study D

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

5

6 of 25

Design Study E

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

6

7 of 25

Design Study F

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

7

8 of 25

Appendix P.4�Numerical Solutions to Differential Equations

8

9 of 25

Numerically Solving an ODE

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

9

10 of 25

Numerically Solving an ODE

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

10

11 of 25

Euler Method / Runge-Kutta 1 (RK1)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

11

12 of 25

Euler Method / Runge-Kutta 1 (RK1)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

12

import numpy as np

class systemDynamics:

# Model the system yddot + a1*ydot + a2*sin(y) = b*u

def __init__(self, Ts=0.01):

self.Ts = Ts # sample rate

# Specify the initial condition

y0 = 0.0 # y at time zero

ydot0 = 0.0 # ydot at time zero

self.state = np.array([[y0], [ydot0]]) # state at time zero

self.a1 = 2.0

self.a2 = 2.0

self.b = 4.0

def update(self, u):

# This is the external method that takes the input u and

# returns the output y.

self.rk1_step(u) # propagate the state by one time sample

y = self.h() # return the corresponding output

return y

def rk1_step(self, u):

# Integrate ODE using Runge-Kutta RK1 algorithm

F1 = self.f(self.state, u)

self.state = self.state + self.Ts * F1

13 of 25

Runge-Kutta 2 (RK2)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

13

14 of 25

Runge-Kutta 2 (RK2)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

14

import numpy as np�

class systemDynamics:

# Model the system yddot + a1*ydot + a2*sin(y) = b*u

def __init__(self, Ts=0.01):

self.Ts = Ts # sample rate

# Specify the initial condition

y0 = 0.0 # y at time zero

ydot0 = 0.0 # ydot at time zero

self.state = np.array([[y0],[ydot0]]) # state at time zero

self.a1 = 2.0

self.a2 = 2.0

self.b = 4.0�

def update(self, u):

# This is the external method that takes the input u and

# returns the output y.

self.rk2_step(u) # propagate the state by one time sample

y = self.h() # return the corresponding output

return y�

def rk2_step(self, u):

# Integrate ODE using Runge-Kutta RK2 algorithm

F1 = self.f(self.state, u)

F2 = self.f(self.state + self.Ts / 2 * F1, u)

self.state += self.Ts / 2 * (F1 + F2)

15 of 25

Runge-Kutta 4 (RK4)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

15

16 of 25

Runge-Kutta 4 (RK4)

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

16

import numpy as np�

class systemDynamics:

# Model the system yddot + a1*ydot + a2*sin(y) = b*u

def __init__(self, Ts=0.01):

self.Ts = Ts # sample rate

# Specify the initial condition

y0 = 0.0 # y at time zero

ydot0 = 0.0 # ydot at time zero

self.state = np.array([[y0],[ydot0]]) # state at time zero

self.a1 = 2.0

self.a2 = 2.0

self.b = 4.0�

def update(self, u):

# This is the external method that takes the input u and

# returns the output y.

self.rk4_step(u) # propagate the state by one time sample

y = self.h() # return the corresponding output

return y�

def rk4_step(self, u):

# Integrate ODE using Runge-Kutta RK4 algorithm

F1 = self.f(self.state, u)

F2 = self.f(self.state + self.Ts / 2 * F1, u)

F3 = self.f(self.state + self.Ts / 2 * F2, u)

F4 = self.f(self.state + self.Ts * F3, u)

self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)

17 of 25

Perturbing a constant

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

17

18 of 25

Perturbing a constant, cont.

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

18

19 of 25

Perturbing a constant, cont.

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

19

20 of 25

Design Study A

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

20

21 of 25

Design Study A

Discuss python implementation

https://github.com/randybeard/controlbook_public/blob/master/_A_arm/python/armDynamics.py

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

21

22 of 25

Design Study B

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

22

\begin{align*}

& \frac{m\ell^2}{3}\ddot{\theta} + mg\frac{\ell}{2}\cos\theta = \tau - b\dot{\theta} \\

\implies & \ddot{\theta} = \frac{3}{m\ell^2} \tau - \frac{3b}{m\ell^2} \dot{\theta} - \frac{3g}{2\ell}\cos\theta

\end{align*}

Define

\[

\mathbf{x} = \begin{pmatrix} x_1 \\ x_2 \end{pmatrix} \defeq \begin{pmatrix} \theta \\ \dot{\theta} \end{pmatrix}

\]

then

\begin{align*}

\dot{\mathbf{x}} &= \begin{pmatrix} \dot{\theta} \\ \ddot{\theta} \end{pmatrix}

= \begin{pmatrix} \dot{\theta} \\ \frac{3}{m\ell^2} \tau - \frac{3b}{m\ell^2} \dot{\theta} - \frac{3g}{2\ell}\cos\theta \end{pmatrix} \\

&= \begin{pmatrix} x_2 \\ \frac{3}{m\ell^2} u - \frac{3b}{m\ell^2} x_2 - \frac{3g}{2\ell}\cos x_1 \end{pmatrix} \\

&\defeq f(\mathbf{x}, u)

\end{align*}

--

Then

\[

\begin{pmatrix} \ddot{z} \\ \ddot{\theta} \end{pmatrix} = M^{-1}(\mathbf{x}) \mathbf{c}(\mathbf{x},u)

\]

and

\begin{align*}

\dot{\mathbf{x}} &= \begin{pmatrix} \dot{z} \\ \dot{\theta} \\ \ddot{z} \\ \ddot{\theta} \end{pmatrix}

= \begin{pmatrix} \dot{z} \\ \dot{\theta} \\ M^{-1}(\mathbf{x}) \mathbf{c}(\mathbf{x}, u) \end{pmatrix} \\

&\defeq f(\mathbf{x}, u)

\end{align*}

23 of 25

Design Study B

Discuss python implementation

https://github.com/randybeard/controlbook_public/blob/master/_B_pendulum/python/pendulumDynamics.py

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

23

24 of 25

Design Study C

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

24

\[

\begin{pmatrix}

J_s & 0 \\

0 & J_p

\end{pmatrix} \begin{pmatrix}\ddot{\theta} \\ \ddot{\phi} \end{pmatrix}

= \begin{pmatrix} \tau-b(\dot{\theta}-\dot{\phi})-k(\theta-\phi) \\ -b(\dot{\phi}-\dot{\theta}) - k(\phi-\theta) \end{pmatrix}.

\]

�

Define

\begin{align*}

\mathbf{x} &\defeq \begin{pmatrix} x_1 \\ x_2 \\ x_3 \\ x_4 \end{pmatrix} = \begin{pmatrix} z \\\theta \\ \dot{z} \\ \dot{\theta} \end{pmatrix} \\

M &\defeq \begin{pmatrix}

J_s & 0 \\

0 & J_p

\end{pmatrix} \\

\mathbf{c}(\mathbf{x}, u) &\defeq \begin{pmatrix} \tau-b(\dot{\theta}-\dot{\phi})-k(\theta-\phi) \\ -b(\dot{\phi}-\dot{\theta}) - k(\phi-\theta) \end{pmatrix}

\end{align*}

--

Then

\[

\begin{pmatrix} \ddot{\theta} \\ \ddot{\phi} \end{pmatrix} = M^{-1} \mathbf{c}(\mathbf{x},u)

\]

and

\begin{align*}

\dot{\mathbf{x}} &= \begin{pmatrix} \dot{\theta} \\ \dot{\phi} \\ \ddot{\theta} \\ \ddot{\phi} \end{pmatrix}

= \begin{pmatrix} x_3 \\ x_4 \\ M^{-1} \mathbf{c}(\mathbf{x}, u) \end{pmatrix} \\

&\defeq f(\mathbf{x}, u)

\end{align*}

25 of 25

Design Study C

Discuss python implementation

https://github.com/randybeard/controlbook_public/blob/master/_C_satellite/python/satelliteDynamics.py

Introduction to Feedback Control, Beard, McLain, Peterson, Killpack

25