#!/usr/bin/env python
# coding: utf-8
# # An overview of rigid body dynamics
# $$
# % vector
# \newcommand{\v}[1]{\mathbf{\vec{#1}}}
# % unit vector
# \newcommand{\u}[1]{\mathbf{\hat{#1}}}
# % dot product
# \newcommand{\dp}[2]{#1 \cdot #2}
# % cross product
# \newcommand{\cp}[2]{#1 \times #2}
# % rotation matrix
# \newcommand{\R}[2]{{}^{#1} R ^{#2}}
# % vector derivative
# \newcommand{\d}[2]{\frac{{}^#2d#1}{dt}}
# % angular velocity
# \newcommand{\av}[2]{{}^{#2} \v{\omega} {}^{#1}}
# % angular acceleration
# \newcommand{\aa}[2]{{}^{#2} \v{\alpha} {}^{#1}}
# % position
# \newcommand{\pos}[2]{\v{r} {}^{#2/#1}}
# % velocity
# \newcommand{\vel}[2]{{}^#2 \v{v} {}^{#1}}
# % acceleration
# \newcommand{\acc}[2]{{}^#2 \v{a} {}^{#1}}
# $$
# Rigid body dynamics is concerned with describing the motion of systems composed of solid bodies; such as vehicles, skeletons, robots [1-4]:
#
# 
# This document borrows heavily from [5, 6].
# # Newton's Second Law
# For all these systems, our goal is to determine how each body's position changes in time. Newton told us that the acceleration $\v{a}$ of a system is proportional to the force $\v{F}$ applied to it:
#
# $$\v{F} = m\v{a}$$
#
# Newton gave us the bodies' acceleration, but we would really prefer to obtain their position. Thus, Newton gave us a second order ordinary differential equation for the quantity we seek, i.e. $\v{a}=\frac{d^2}{dt^2}\v{x}$. This equation is actually far too simple for the systems we want to study, but it reveals that there are three topics to consider in describing a rigid body system: its kinematics ($\v{a}$), its mass properties ($m$), and the external forces applied to the system ($\v{F}$). In this notebook, we present the tools necessary to mathematically describe a rigid body system. Once equipped with a mathematical description of a system, we can generate equations that describe its motion. Regardless, we always end up with second-order ordinary differential equations in time.
# # Vectors
# Newton's second law is a vector equation. A vector is a quantity that has a **magnitude** and a **direction**. For example, "5 miles East" is a vector quantity with magnitude 5 miles and direction East. We draw them as arrows:
#
# 
#
# We represent the magnitude of a vector $\v{v}$ as $|\v{v}|$. We represent the direction of a vector $\v{v}$ using a unit vector $\u{u}$ (magnitude of 1) that has the same direction as $\v{v}$:
#
# $$\u{u} = \frac{\v{v}}{|\v{v}|}$$
# We will work with the following vector quantities: positions, velocities, accelerations, forces, angular velocities, and torques/moments. Don't think about these vectors as linear algebra vectors. Our vectors always have a physical interpretation (and thus are always 2 or 3 dimensinonal), while linear algebra vectors are often more abstract.
# ## Addition
# When we add vector $\v{b}$ to vector $\v{a}$, the result is a vector that starts at the tail of $\v{a}$ and ends at the tip of $\v{b}$:
#
# 
# In[ ]:
from __future__ import print_function, division
from sympy import init_printing
init_printing(use_latex='mathjax', pretty_print=False)
# Physics vectors in SymPy are created by first specifying a reference frame and using the associated unit vectors to construct vectors of arbritrary magnitude and direction. Reference frames will be discussed later on and for now it is only important that `N.x`, `N.y`, and `N.z` are three mutally orthoganl vectors of unit length.
# In[ ]:
from sympy.physics.vector import ReferenceFrame
N = ReferenceFrame('N')
# Simple scalar variables can be import from SymPy with:
# In[ ]:
from sympy.abc import c, d, e, f, g, h
# Finally, the unit vectors and scalars can be combined to create vectors.
# In[ ]:
a = c * N.x + d * N.y + e * N.z
a
# In[ ]:
a.to_matrix(N)
# In[ ]:
b = f * N.x + g * N.y + h * N.z
b
# The magnitude of a vector can be found with::
# In[ ]:
a.magnitude()
# In[ ]:
a + b
# ## Scaling
# Multiplying a vector by a scalar changes its magnitude, but not its direction. Scaling by a negative number changes a vector's magnitude and reverses its sense (rotates it by 180 degrees).
#
# 
# In[ ]:
b = 2 * a
b
# In[ ]:
c = -a
c
# ### Exercise
#
# Create three vectors that lie in the $XY$ plane where each vector is:
#
# 1. of length $l$ that is at an angle of $\frac{\pi}{4}$ degrees from the $X$ axis
# 2. of length $10$ and is in the $-Y$ direction
# 3. of length $l$ and is $\theta$ radians from the $Y$ axis
#
# Finally, add vectors 1 and 2 and substract $5$ times the third vector.
#
# *Hint: SymPy has variables and trigonometic functions, for example `from sympy import tan, pi`*
# In[ ]:
# In[ ]:
get_ipython().run_line_magic('load', 'exercise_solutions/n01_vector_addition_scaling.py')
# ## Dot product (scalar product)
# The dot product, which yields a scalar quantity, is defined as:
#
# $$\v{a} \cdot \v{b} = |\v{a}||\v{b}| \cos{\theta}$$
#
# where $\theta$ is the angle between the two vectors. It is used to determine:
#
# * the angle between two vectors: $\theta = \cos^{-1}[\v{a} \cdot \v{b} / (|\v{a}||\v{b}|)]$
# * a vector's magnitude: $ |\v{v}| = \sqrt{\v{v} \cdot \v{v}} $
# * the length of a vector along a direction/unit vector $\u{u}$ (called the projection): $ \mbox{proj}_{\u{u}}\v{v} = \v{v} \cdot \u{u}$
# * if two vectors are perpendicular: $ \v{a} \cdot \v{b} = 0 \mbox{ if } \v{a} \perp \v{b} $
# * compute power: $ P = \dp{\v{F}}{\v{v}}$
#
# Also, dot products are used to convert a vector equation into a scalar equation (by "dotting" an entire equation with a vector).
#
# 
# In[ ]:
from sympy.abc import c, d, e, f, g, h
from sympy.physics.vector import ReferenceFrame, dot
# In[ ]:
N = ReferenceFrame('N')
a = c * N.x + d * N.y + e * N.z
b = f * N.x + g * N.y + h * N.z
# In[ ]:
dot(a, b)
# ### Exercise
#
# Given the vectors $\v{v}_1 = a \hat{\mathbf{n}}_x + b\hat{\mathbf{n}}_y + a \hat{\mathbf{n}}_z$ and $\v{v}_2=b \hat{\mathbf{n}}_x + a\hat{\mathbf{n}}_y + b \hat{\mathbf{n}}_z$ find the angle between the two vectors using the dot product.
# In[ ]:
# In[ ]:
get_ipython().run_line_magic('load', 'exercise_solutions/n01_vector_dot_product.py')
# ## Cross product (vector product)
# The cross product, which yields a vector quantity, is defined as:
#
# $$ \cp{\v{a}}{\v{b}} = |\v{a}||\v{b}| \sin\theta \u{u}$$
#
# where $\theta$ is the angle between the two vectors, and $\u{u}$ is the unit vector perpendicular to both $\v{a}$ and $\v{b}$ whose sense is given by the right-hand rule. It is used to:
#
# * obtain a vector/direction perpendicular to two other vectors
# * determine if two vectors are parallel: $\cp{\v{a}}{\v{b}} = \v{0} \mbox{ if } \v{a} \parallel \v{b}$
# * compute moments: $ \cp{\v{r}}{\v{F}}$
# * compute the area of a triangle
#
# 
# In[ ]:
from sympy.abc import c, d, e, f, g, h
from sympy.physics.vector import ReferenceFrame, cross
# In[ ]:
N = ReferenceFrame('N')
a = c * N.x + d * N.y + e * N.z
b = f * N.x + g * N.y + h * N.z
# In[ ]:
cross(a, b)
# ### Exercise
#
# Given three points located in reference frame $N$ by:
#
# $$
# \v{p}_1 = 23 \u{n}_x - 12 \u{n}_y \\
# \v{p}_2 = 16 \u{n}_x + 2 \u{n}_y - 4 \u{n}_z \\
# \v{p}_3 = \u{n}_x + 14 \u{n}_z
# $$
#
# Find the area of the triangle bounded by these three points using the cross product.
#
# *Hint: Search online for the relationship of the cross product to triangle area.*
# In[ ]:
# In[ ]:
get_ipython().run_line_magic('load', 'exercise_solutions/n01_vector_cross_product.py')
# ## Some vector properties
# * The order in which you add them does not matter: $\v{a} + \v{b} = \v{b} + \v{a}$
# * You can distrubute a scalar among vectors: $ s (\v{a} + \v{b}) = s\v{a} + s\v{b} $
#
# **Dot product**
#
# * You can pull out scalars: $ c \v{a} \cdot d \v{b} = cd (\v{a} \cdot \v{b})$
# * Order does not matter: $\dp{\v{a}}{\v{b}} = \dp{\v{b}}{\v{a}}$
# * You can distribute: $\dp{\v{a}}{(\v{b} + \v{c})} = \dp{\v{a}}{\v{b}} + \dp{\v{a}}{\v{c}}$
#
# **Cross product**
#
# * Crossing a vector with itself "cancels" it: $\cp{\v{a}}{\v{b}} = \vec{0}$
# * You can pull out scalars: $ c \v{a} \times d \v{b} = cd (\v{a} \times \v{b})$
# * Order DOES matter (because of the right-hand rule): $\cp{\v{a}}{\v{b}} = -\cp{\v{b}}{\v{a}}$
# * You can distribute: $\cp{\v{a}}{(\v{b} + \v{c})} = \cp{\v{a}}{\v{b}} + \cp{\v{a}}{\v{c}}$
# * They are NOT associative: $\cp{\v{a}}{(\cp{\v{b}}{\v{c}})} \neq \cp{(\cp{\v{a}}{\v{b}})}{\v{c}}$
# # Reference frames
# A reference frame (or simply, frame) is a rigid 3D object. We always attach a reference frame to rigid bodies in order to describe their motion. We may also use "empty" reference frames to make a system easier to model.
#
# A reference frame has some *location* in space, but it does *not* have a position. Reference frames contain points, and those *points* have positions.
#
# A reference frame also has an *orientation* in space. To specify its orientation, we choose a vector basis whose orientation is fixed with respect to the reference frame (but there are infinitely many vector bases we *could* label on the frame). In general, we are only interested in the vector bases we attach to reference frames; from here on, we will instead refer to reference frames in the places where we referred vector bases. That is, we express vectors in a reference frame instead of in a vector basis.
#
# A reference frame's location and orientation vary in time. Two important attributes of a reference frame are its **angular velocity** $\v{\omega}$ and its **angular acceleration** $\v{\alpha}$; we'll describe these shortly.
#
# A **Newtonian reference frame** is one in which Newton's second law holds.
#
# 
# ## Expressing vectors with a vector basis
# We have shown you what a vector $\v{v}$ looks like, but have yet to express an actual vector mathematically. To do so, we first choose three unit vectors $\u{a}_x$, $\u{a}_y$, and $\u{a}_z$ whose directions we accept as given. Consider the human jumper from above; we choose:
#
# * $\u{a}_x$ to point forward,
# * $\u{a}_y$ to point upwards,
# * $\u{a}_z$ to point out of the plane (to the subject's right).
#
# 
#
# These three unit vectors are mutually perpendicular. For pratical reasons, we will always make sure that's the case. If so, the three vectors define a vector basis. We can express the position of the subject's hand from its toes in terms of these three vectors:
#
# $$ \v{r} = d_x \u{a}_x + d_y \u{a}_y + 0 \u{a}_z$$
#
# We call $d_x$ the **measure** of $\v{r}$ along $\u{a}_x$, and it is equal to $\v{r} \cdot \u{a}_x$. Note that a vector basis does not have an origin.
#
# We could have chosen a different vector basis, such as $\u{b}_x$, $\u{b}_y$, $\u{b}_z$. Then, we would express $\v{r}$ as:
#
# $$ \v{r} = f_x \u{b}_x + f_y \u{b}_y + 0 \u{b}_z$$
#
# Using this alternative vector basis does not change the fact that $\v{r}$ is the position of the hand from the toes; it simply changes how we *express* this quantity. It is possible to express a single vector in infinitely many ways, since we can choose to use any valid vector basis. In the next section, we will learn how to relate different vector bases to each other.
#
# #### Operating on vectors expressed in a basis
# Once we express a vector in a vector basis, it is easy to perform operations on it with vectors expressed in the same basis. Consider the two vectors:
#
# * $\v{a} = a_x \u{n}_x + a_y \u{n}_y + a_z \u{n}_z$
# * $\v{b} = b_x \u{n}_x + b_y \u{n}_y + b_z \u{n}_z$
#
# Here are the addition, dot, and cross operations between these two vectors:
#
# $$
# \v{a} + \v{b} = (a_x + b_x) \u{n}_x + (a_y + b_y) \u{n}_y + (a_z + b_z) \u{n}_z \\
# \dp{\v{a}}{\v{b}} = a_x b_x + a_y b_y + a_z b_z\\
# \cp{\v{a}}{\v{b}} = \det{
# \begin{bmatrix} \u{n}_x & \u{n}_y & \u{n}_z \\
# a_x & a_y & a_z \\
# b_x & b_y & b_z
# \end{bmatrix}}
# $$
#
# #### We must specify a vector basis
# When a vector is expressed in typical linear algebra notation, information is lost. For example, we don't know the basis in which the following vector is expressed:
#
# $$
# \v{v} = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix}
# $$
#
# If we don't know the basis in which $v_x$, $v_y$, and $v_z$ are its measures, we cannot add $\v{v}$ to another vector, etc. To express a vector in matrix form, we must carry along the basis in which it is expressed. One option for doing so is the following:
#
# $$
# [\v{v}]_{n} = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix}_{n} $$
#
# The notation $[\v{v}]_{n}$ specifies that $\v{v}$ is expressed in the vector basis $\u{n}_x$, $\u{n}_y$, $\u{n}_z$.
# In[ ]:
from sympy.abc import c, d, e, f, g, h, theta
from sympy.physics.vector import ReferenceFrame, dot, cross
# In[ ]:
A = ReferenceFrame('A')
# In[ ]:
B = A.orientnew('B', 'Axis', (theta, A.z))
# In[ ]:
a = c * A.x + d * A.y + e * A.z
b = f * B.x + g * B.y + h * B.z
# In[ ]:
a + b
# In[ ]:
dot(a, b)
# In[ ]:
cross(a, b)
# In[ ]:
(a+b).express(A)
# # Rotation matrices (direction cosine matrices)
# In almost every problem, we make use of multiple vector bases. The reason is that there is usually a particular basis in which a vector is most conveniently expressed. And, that convenient basis is usually not the same for all vectors we'll deal with. A side effect is that we will often want to change the basis in which a vector is expressed. To do so, we use a rotation matrix (also called a direction cosine matrix). The rotation matrix ${}^a R^b$ allows us to take a vector $\v{v}$ expressed in $\u{b}_x$, $\u{b}_y$, $\u{b}_z$ and re-express it in $\u{a}_x$, $\u{a}_y$, $\u{a}_z$:
#
# $$
# [\v{v}]_{a} = {}^a R^b ~ [\v{v}]_{b}
# $$
#
# The rotation matrix is given by dot products across the two the vector bases:
#
# $$
# \R{a}{b} =
# \begin{bmatrix}
# \dp{\u{a}_x}{\u{b}_x} & \dp{\u{a}_x}{\u{b}_y} & \dp{\u{a}_x}{\u{b}_z} \\
# \dp{\u{a}_y}{\u{b}_x} & \dp{\u{a}_y}{\u{b}_y} & \dp{\u{a}_y}{\u{b}_z} \\
# \dp{\u{a}_z}{\u{b}_x} & \dp{\u{a}_z}{\u{b}_y} & \dp{\u{a}_z}{\u{b}_z} \\
# \end{bmatrix}
# $$
#
# Because of the nature of vector bases, this matrix is symmetric and orthogonal. If we instead have a vector in basis $a$ and want to express it in $b$, we can simply use the inverse of $\R{a}{b}$. Since the matrix is orthogonal, its inverse is the same as its transpose.
#
# $$
# \R{b}{a} = (\R{a}{b})^{-1} = (\R{a}{b})^T \\
# [\v{v}]_{b} = {}^b R^a ~ [\v{v}]_{a} \\
# [\v{v}]_{b} = ({}^a R^b)^T ~ [\v{v}]_{a}
# $$
#
# The columns of $\R{a}{b}$ are the unit vectors $\u{b}_x$, $\u{b}_y$, $\u{b}_z$ expressed in $a$:
#
# $$
# \R{a}{b} = \begin{bmatrix} [\u{b}_x]_a & [\u{b}_y]_a & [\u{b}_z]_a \end{bmatrix}
# $$
#
# #### Successive rotations
#
# We'll usually need to re-express a vector multiple times. Luckily, we can do so by multiplying rotation matrices together:
#
# $$
# \R{d}{a} = (\R{d}{c} )(\R{c}{b}) (\R{b}{a}) \\
# [\v{v}]_{d} = \R{d}{a} [\v{v}]_{a} \\
# [\v{v}]_{d} = (\R{d}{c} )(\R{c}{b}) (\R{b}{a})[\v{v}]_{a}
# $$
#
# #### A point of confusion: rotating vs. re-expressing
#
# Sometimes, rotation matrices are used to rotate vectors; that is, cause the vector to point somewhere different. That is NOT how we are using rotation matrices here. Rotating a vector changes the vector itself, while we are only changing how the *same* vector is expressed.
# In[ ]:
B.dcm(A)
# ### Exercise
#
# Create two reference frames, the first should be attached to your laptop keyboard surface. For the first frame, the $Z$ axis should be directed from the Q key to the P key. The $Y$ unit vector should be directed from the shift key to the tab key. Now on the screen, attach a reference frame where the $Z$ axis is directed from the right side of the screen to the left and lies in the plane of the screen. The $Y$ axis should be directed from the top of the screen to the hinge.
#
# The angle between the laptop and screen is $\theta$ such that $\theta=0$ corresponds to the laptop being closed and $0 < \theta < \pi$ is the laptop being open. With this create a vector that starts at the bottom left hand corner of the wrist rests and ends at the top right corner of the screen. Use $w$ for the width and $l$ for the length of the laptop.
#
# Print the vector expressed in the keyboard frame.
#
# *Hint: You may need to create more than two frames and a simple sketch will help.*
# In[ ]:
# In[ ]:
get_ipython().run_line_magic('load', 'exercise_solutions/n01_vector_rotation.py')
# # Derivatives of vectors
# Consider the vector $\u{a}_x$ in the figure above. To an observer sitting on $A$, $\u{a}_x$ never changes; it is fixed rigidly to $A$. Therefore, the observer would say the time derivative of $\u{a}_x$ is $\v{0}$. However, an observer on $N$ would indeed observe that $\u{a}_x$ changes in time. For this reason, when we take the time derivative of a vector, we must specify the frame in which we take the derivative. The derivative of a generic vector $\v{p}$ in frame $N$ is denoted as:
#
# $$\d{\v{p}}{N}$$
#
# Consider a vector $\v{p}$ expressed in $A$:
#
# $$\v{p} = p_x \u{a}_x + p_y \u{a}_y + p_z \u{a}_z$$
#
# Its time derivative in frame $A$ is:
#
# $$\d{\v{p}}{A} = \dot{p}_x \u{a}_x + \dot{p}_y \u{a}_y + \dot{p}_z \u{a}_z$$
#
# Here, we have benefitted from the fact that $\u{a}_x$, $\u{a}_y$, and $\u{a}_z$ are constant in $A$. We are not so fortunate when taking the derivative in $N$, since these basis vectors are not constant in $N$:
#
# $$\d{\v{p}}{N} = \dot{p}_x \u{a}_x + p_x \d{\u{a}_x}{N} + \dot{p}_y \u{a}_y + p_y \d{\u{a}_y}{A} + \dot{p}_z \u{a}_z + p_z \d{\u{a}_z}{N}$$
#
# This formula for the derivative in $N$ of a vector expressed in $A$ is not so great to use. Once we introduce angular velocity, we will have a much better way to compute such quantities.
# In[ ]:
a
# In[ ]:
a.diff(c, A)
# # Angular velocity and angular acceleration
# A reference frame's angular velocity describes the rate of change of the frame's orientation. Consider frame $A$. Since angular velocity is a vector quantity, we must specify the frame from which we observe the change in $A$'s orientation.
#
#
# $\av{A}{N}$: the angular velocity of frame $A$ as observed from frame $N$
#
#
# There are some complicated formulas for $\av{A}{N}$, but you usually don't need them. Typically, you know $\av{A}{N}$ by inspection. Take the linkage below:
#
# 
#
# In this linkage, the only way that frame/body $B$ can move with respect to $A$ is by rotating about $B_o$ by the angle $q_1$. Thus, by inspection:
#
# $$\av{B}{A} = \dot{q}_1 \u{b}_z$$
#
# $C$ is attached to $B$ similarly:
#
# $$\av{C}{B} = \dot{q}_2 \u{c}_z$$
#
# #### Angular velocity addition theorem
#
# We can add angular velocities together, similar to how we multiplied reference frames:
#
# $$\av{C}{A} = \av{B}{A} + \av{C}{B}$$
#
# #### Derivative theorem
#
# For any vector $\v{p}$, the following equation relates the derivative of $\v{p}$ in two different reference frames via the angular velocity between these two frames:
#
# $$\d{\v{p}}{A} = \d{\v{p}}{B} + \av{B}{A} \times \v{p}$$
#
# Again, this works for *any* vector, not just position vectors.
# This theorem is really important, and is the primary way that we compute derivatives of vectors in other frames.
#
# #### Angular acceleration
#
# The equations of rigid body dynamics will also require angular accelerations $\aa{B}{A}$ of the rigid bodies in the system, but this can usually be computed automatically from $\av{B}{A}$.
#
#
# $\aa{A}{N}$: the angular acceleration of frame $A$ as observed from frame $N$
#
# In[ ]:
B.ang_vel_in(A)
# In[ ]:
from sympy import Function
from sympy.abc import t
theta = Function('theta')(t)
theta
# In[ ]:
theta.diff(t)
# In[ ]:
B = A.orientnew('B', 'Axis', (theta, A.z))
B.ang_vel_in(A)
# # Position, velocity, and acceleration
# #### Position
#
# Position vectors have the special property that two points must be specified. For example, if I want to obtain the position of point $P$ in the figure above, I must specify the point from which I want that position.
#
#
# $\pos{Q}{P}$: the position of point $Q$ with respect to point $P$.
#
#
# In modeling, we often must write down various position vectors via inspection.
#
# #### Velocity
#
# The velocity of a point is the derivative of its position, and must have associated with it the frame in which the derivative is taken.
#
#
# $\vel{Q}{N}$: the velocity of point $Q$ in frame $N$
#
#
# Previously, we used the symbol $\v{v}$ to denote a generic vector. Henceforth, $\v{v}$ refers to a velocity. If $N_o$ is a point fixed in $N$, then:
#
# $$\vel{Q}{N}=\d{\pos{Q}{N_o}}{N}$$
#
# When using PyDy, we rarely need to use inspection to determine the velocity of points of interest. Instead, we are usually in the situation that we want the velocity (in $N$) of point $Q$ fixed on body $B$, and we already know the velocity of another point $P$ fixed on $B$. In this case, we use the following formula to obtain $\vel{Q}{N}$ (`v2pt_theory` in PyDy):
#
# $$\vel{Q}{N} = \vel{P}{N} + \av{B}{N} \times \pos{Q}{P}$$
#
# #### Acceleration
#
# An acceleration of a point is the derivative of its velocity, and must have associated with it the frame in which the derivative is taken.
#
#
# $\acc{Q}{N}$: the acceleration of point $Q$ in frame $N$
#
#
# Henceforth, $\v{a}$ refers to an acceleration. If the velocity of $Q$ is given, then the acceleration is:
#
# $$\acc{Q}{N}=\d{\vel{Q}{N}}{N}$$
#
# Similarly to velocity, we rarely need to use inspection to determine the acceleration of points of interest. Instead, we are usually in the situation that we want the acceleration (in $N$) of point $Q$ fixed on body $B$, and we already know the velocity of another point $P$ fixed on $B$. In this case, we use the following formula to obtain $\acc{Q}{N}$ (`a2pt_theory` in PyDy):
#
# $$\acc{Q}{N} = \acc{P}{N} + \aa{B}{N} \times \pos{Q}{P} + \av{B}{N} \times (\av{B}{N} \times \pos{Q}{P})$$
# # Inertial properties
# Each particle or rigid body has interial properties. We will assume that these properties are constant with respect to time. Each particle in a system has a scalar mass and each rigid body has a scalar mass located at it's center of mass and an inertia dyadic (or tensor) that represents how that mass is distributed in space, which is typically defined with respect to the center of mass.
#
# Just as we do with vectors above, we will use a basis dependent expression of tensors. The inertia of a 3D rigid body is typically expressed as a tensor (symmetric 3 x 3 matrix).
#
# $$
# I = \begin{bmatrix}
# I_{xx} & I_{xy} & I_{xz} \\
# I_{xy} & I_{yy} & I_{yz} \\
# I_{xz} & I_{yz} & I_{zz}
# \end{bmatrix}_N
# $$
#
# The three terms on the diagnol are the moments of inertia and represent the resistance to angular acceleration about the respective axis in the subscript. The off diagonal terms are the products of inertia and represent the coupled resistance to angular acceleration from one axis to another. The $N$ subscript denotes that this tensor is expressed in the $N$ reference frame.
#
# We can write this tensor as a dyadic to allow for easy combinations of inertia tensors expressed in different frames, just like we combine vectors expressed in different frames above. This basis dependent tensor takes the form:
#
# $$I =
# I_{xx} \begin{bmatrix}
# 1 & 0 & 0 \\
# 0 & 0 & 0 \\
# 0 & 0 & 0
# \end{bmatrix}_N +
# I_{xy} \begin{bmatrix}
# 0 & 1 & 0 \\
# 0 & 0 & 0 \\
# 0 & 0 & 0
# \end{bmatrix}_N +
# I_{xz} \begin{bmatrix}
# 0 & 0 & 1 \\
# 0 & 0 & 0 \\
# 0 & 0 & 0
# \end{bmatrix}_N +
# I_{yx} \begin{bmatrix}
# 0 & 0 & 0 \\
# 1 & 0 & 0 \\
# 0 & 0 & 0
# \end{bmatrix}_N +
# I_{yy} \begin{bmatrix}
# 0 & 0 & 0 \\
# 0 & 1 & 0 \\
# 0 & 0 & 0
# \end{bmatrix}_N +
# I_{yz} \begin{bmatrix}
# 0 & 0 & 0 \\
# 0 & 0 & 1 \\
# 0 & 0 & 0
# \end{bmatrix}_N + \\
# I_{zx} \begin{bmatrix}
# 0 & 0 & 0 \\
# 0 & 0 & 0 \\
# 1 & 0 & 0
# \end{bmatrix}_N +
# I_{zy} \begin{bmatrix}
# 0 & 0 & 0 \\
# 0 & 0 & 0 \\
# 0 & 1 & 0
# \end{bmatrix}_N +
# I_{zz} \begin{bmatrix}
# 0 & 0 & 0 \\
# 0 & 0 & 0 \\
# 0 & 0 & 1
# \end{bmatrix}_N
# $$
#
# These "unit" tensors are simply the outer product of the associated unit vectors and can be written as such:
#
# $$ I = I_{xx} \u{n}_x \otimes \u{n}_x +
# I_{xy} \u{n}_x \otimes \u{n}_y +
# I_{xz} \u{n}_x \otimes \u{n}_z +
# I_{yx} \u{n}_y \otimes \u{n}_x +
# I_{yy} \u{n}_y \otimes \u{n}_y +
# I_{yz} \u{n}_y \otimes \u{n}_z +
# I_{zx} \u{n}_z \otimes \u{n}_x +
# I_{zy} \u{n}_z \otimes \u{n}_y +
# I_{zz} \u{n}_z \otimes \u{n}_z $$
# Inertia dyadics and tensors can be created in the following way:
# In[ ]:
from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame, inertia
# In[ ]:
ixx, iyy, izz, ixy, iyz, ixz = symbols('I_xx I_yy I_zz I_xy I_yz I_xz')
# In[ ]:
N = ReferenceFrame('N')
# In[ ]:
I = inertia(N, ixx, iyy, izz, ixy, iyz, ixz)
I
# In[ ]:
I.to_matrix(N)
# # Forces and moments/torques
# Forces are vectors which are applied to specific points (bound vectors) and moments/torques are vectors than describe rotational load applied to a body. Both can simply be described as vectors but either a point or reference frame must be associated with each, respectively.
#
# **Equal and Opposite**
#
# Don't forget Newton's third law of motion. If there is a force or torque, there is always an equal and opposit force or torque acting on the opposing point or reference frame.
# In[ ]:
from sympy.abc import a, b, c
from sympy.physics.vector import ReferenceFrame, Point
# In[ ]:
A = ReferenceFrame('A')
P = Point('P')
f = a * A.x + b * A.y + b * A.z
# We will typically denote a force as tuple of a vector and a point.
force = (f, P)
f
# # Equations of motion
# Once all of the important forces acting on a system, the accelerations of all particles and bodies, and the inertial properties of the system are found, the equations of motion can be formed. For planar dyanmics the equations take on this form:
#
# $$\sum \v{F} = m \v{a} \\ \sum \v{T} = I \v{\alpha}$$
#
# The force equation (Newton's second law) and the torque equation (Euler's equation) make up a set of second ordinary differential equations in time. We typically want these equations in first order form:
#
# $$\dot{x} = f(x, u, t)$$
#
# And to do that kinematical differential equations are introduced, which simply define the relationships between the positional and angular states and their derivatives. For example, we could introduce $\omega$ as the time derivative of an angle $\theta$:
#
# $$\omega = \dot{\theta}$$
#
# The states $x$ are typically positions, angles, and their rates.
# In general, the equations of motion are non-linear ordinary differential equations and anayltical solutions do not exist. To find the resulting state trajectories we turn to numerical integration methods.
# $$x = \int_{t_0}^{t_f} f(x, u, t) dt$$
# # References
# [1] Moore, J. K. (2012). Human Control of a Bicycle. University of California, Davis.
#
# [2] Ashby, B. M., & Delp, S. L. (2006). Optimal control simulations reveal mechanisms by which arm movement improves standing long jump performance. Journal of biomechanics, 39(9), 1726–34. doi:10.1016/j.jbiomech.2005.04.017
#
# [3] Gong, X., Bai, Y., Hou, Z., Zhao, C., Tian, Y., & Sun, Q. (2012). Backstepping sliding mode tracking control of quad-rotor under input saturation. International Journal of Intelligent Computing and Cybernetics, 5(4), 515–532. doi:10.1108/17563781211282268
#
# [4] Wu, J. Z., An, K.-N., Cutlip, R. G., Krajnak, K., Welcome, D., & Dong, R. G. (2008). Analysis of musculoskeletal loading in an index finger during tapping. Journal of biomechanics, 41(3), 668–76. doi:10.1016/j.jbiomech.2007.09.025
#
# [5] Mitiguy, P. Advanced Dynamics & Motion Simulation. 2013.
#
# [6] http://docs.sympy.org/latest/modules/physics/mechanics/index.html