# Introduction

The first step, and notably the most difficult, is to define the kinematic relationships (i.e. motion) among rigid bodies in the system. Here will we make use of `ReferenceFrame` objects to describe the four frames in the problem, set their orientations, and then construct vectors in the frames that position various important `Point`s. Finally, we will specify the linear and angular velocities of the frames and points using generalized speeds.

# Setup

First, import the necessary functions from SymPy that will allow us to construct time varying vectors in the four reference frames.


```python
from __future__ import print_function, division
from sympy import symbols, simplify
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
```

SymPy has a rich printing system. Here we initialize printing so that all of the mathematical equations are rendered in standard mathematical notation.


```python
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
```

The IPython notebook can display rich content. We will use the Image function to import some images for reference.


```python
from IPython.display import Image
```

# Reference Frames and Orientation

Our first step is to define three reference frames, one each for the lower leg, upper leg, and torso. These reference frames hold the information that defines how each frame is oriented relative to each other in addition to their relative angular velocites and angular accelerations. We start by creating four reference frames for each frame in the system. The initialization of the torso frame is left for a later exercise.


```python
inertial_frame = ReferenceFrame('I')
```


```python
lower_leg_frame = ReferenceFrame('L')
```


```python
upper_leg_frame = ReferenceFrame('U')
```

Now we need to specify how the frames are oriented with respect to each other. To do this, we need to define the three generalized coordinates, $\theta_1(t)$, $\theta_2(t)$, and $\theta_3(t)$, for the ankle, knee, and hip angles which are time varying quantities.


```python
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
```

We can start by setting the orientation of the lower leg relative to the inertial reference frame. We'd like the lower leg to rotate through the angle $\theta_1$ relative to the inertial frame. $\theta_1$ is the plantarflexion angle of the ankle. The `ReferenceFrame.orient()` method makes this easy. Simply give the base frame, `Axis` type of rotation, and a tuple containing the rotation angle, and the vector about which to rotate, in this case the Z unit vector of the inertial reference frame.


```python
Image('figures/ankle_joint.png')
```


```python
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
```

We now can see that the direction cosine matrix relating the two reference frames has been set on both frames.


```python
lower_leg_frame.dcm(inertial_frame)
```


```python
inertial_frame.dcm(lower_leg_frame)
```

We can rotate the upper leg relative to the lower leg through the knee flexion angle, $\theta_2$, in a similar fashion. 


```python
Image('figures/knee_joint.png')
```


```python
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z))
```

Now that we have two reference frames chained, we can show the direction cosine matrices of the uppper leg (one end of the chain) to the inertial reference frame (the other end of the chain).


```python
upper_leg_frame.dcm(inertial_frame)
```

Notice that thedirection cosine matrix isn't in its simplest form. We can be simplified with using the `simplify()` function.


```python
simplify(upper_leg_frame.dcm(inertial_frame))
```

## Exercise

Now as an excercise, define a `torso_frame` with the name `T` and rotate it relative to the upper leg through the hip extension angle, $\theta_3$. Lastly show the simplified direction cosine matrix of the torso with respect to the inertial reference frame.


```python
Image('figures/hip_joint.png')
```


```python
# Enter and run your answer here

```


```python
%load exercise_solutions/n03_kinematics_rotate-frame.py
```

# Point and Locations

To derive the equations of motion of a system we need to have defined the velocities of the mass centers of each rigid body. We will first define points that locate the joints: ankle, knee, and hip to simplify the mass centers of the lower leg, upper leg, and torso.

## Joints

The ankle will be our based point and all other point will be defined with respect to the ankle. The ankle will be considered fixed in the inertial reference frame, i.e. $^I\mathbf{v}^A=0$


```python
ankle = Point('A')
```

The knee is defined with respect to the ankle with a vector that is the length from the ankle to the knee, $l_L$, in the direction of the lower leg $\hat{\mathbf{l}}_y$ unit vector. We need the constant length and a new point $K$.


```python
lower_leg_length = symbols('l_L')
```


```python
knee = Point('K')
```

The position is set with the `Point.set_pos()` method, giving the reference point and the vector.


```python
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
```

The position from any other point can then be found using the `Point.pos_from()` method.


```python
knee.pos_from(ankle)
```

It is also possible to express the returned vector in another reference frame. For example, you may want to know the position of the knee in the coordinates of the inertial reference frame.


```python
knee.pos_from(ankle).express(inertial_frame).simplify()
```

In a similar fashion the hip is defined with respect to the knee.


```python
upper_leg_length = symbols('l_U')
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
hip.pos_from(ankle)
```

The vector from the ankle to the hip point can be expressed in the inertial reference frame with:


```python
hip.pos_from(ankle).express(inertial_frame).simplify()
```

## Center of mass locations

### Exercise

The mass centers of the three bodies can be located in a similar fashion. The distances $d_L,d_U,d_T$ locate the mass centers relative to the distal end of the body segments. First define some new symbols:


```python

```

Now define the point $L_o$ and set the position of the `lower_leg_mass_center`.


```python

```

Position the `upper_leg_mass_center` ($U_o$).


```python

```

Position the `torso_mass_center` ($T_o$).


```python

```


```python
%load exercise_solutions/n03_kinematics_define-com.py
```

# Kinematical Differential Equations

At this point, we are going to select three generalized speeds, $\omega_1$, $\omega_2$, and $\omega_3$, (the angular velocities of the joints) so that the time derivative of the generalized coordinates are equal to the genearlized speeds. These relationships are called the kinematical differential equations and allow for the remaining equations of motion to easily be derived in first order form. This step is explicit when using Kane's method, which we are going to use. First create the time varying symbols:


```python
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
```

To enforce the relationships: $\omega_n = \dot{\theta}_n$ we define these three equations $\omega_n - \dot{\theta}_n = 0$: 


```python
kinematical_differential_equations = [omega1 - theta1.diff(),
                                      omega2 - theta2.diff(),
                                      omega3 - theta3.diff()]
kinematical_differential_equations
```

# Angular Velocities

### Exercise

Now we can use the generalized speeds to define the angular velocities of the reference frames. Due to our definitions of rotations these are simply $\omega_n \hat{\mathbf{k}}$.

Hint: Remember how we located the joint centers and center of mass locations. The syntax is very similar here.

Set the angular velocity of the `lower_leg_frame`.


```python
lower_leg_frame.set_ang_vel(inertial_frame,omega1*inertial_frame.z)
```


```python
lower_leg_frame.ang_vel_in(inertial_frame)
```

Set the angular velocity of the `upper_leg_frame`.


```python

```

Set the angular velocity of the `torso_frame`.


```python

```


```python
%load exercise_solutions/n03_kinematics_define-angular-velocity.py
```

# Linear Velocities

Finally, the linear velocities of the mass centers are needed. Starting at the ankle which has a velocity of 0.


```python
ankle.set_vel(inertial_frame, 0)
```

Working our way up the chain we can make use of the fact that the joint points are located on two rigid bodies. Any fixed point in a reference frame can be computed if the linear velocity of another point on that frame is known and the frame's angular velocity is known.

$$^I\mathbf{v}^{P_2} = ^I\mathbf{v}^{P_1} + ^I\omega^A \times \mathbf{r}^{\frac{P_2}{P_1}}$$

The `Point.v2pt_theory()` method makes it easy to do this calculation.


```python
lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
lower_leg_mass_center.vel(inertial_frame)
```


```python
knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
knee.vel(inertial_frame)
```


```python
upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame)
upper_leg_mass_center.vel(inertial_frame)
```


```python
hip.v2pt_theory(knee, inertial_frame, upper_leg_frame)
hip.vel(inertial_frame)
```


```python
torso_mass_center.v2pt_theory(hip, inertial_frame, torso_frame)
torso_mass_center.vel(inertial_frame)
```

bbf6d1710ab5687d9fb7d55096742f9a43e777b1
ipynb
Jupyter Notebook
notebooks/n03_kinematics.ipynb
pydy/pydy-tutorial-human-standing
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-05-19T15:24:18.000Z
2022-03-12T09:39:03.000Z
notebooks/n03_kinematics.ipynb
pydy/pydy-tutorial-human-standing
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-05-05T18:08:20.000Z
2022-01-28T11:12:42.000Z
notebooks/n03_kinematics.ipynb
pydy/pydy-tutorial-pycon-2014
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-06-16T01:50:51.000Z
2022-02-26T07:39:41.000Z
Qwen/Qwen-72B
1. YES
2. YES