#!/usr/bin/env python
# coding: utf-8

# In this tutorial we are going to make a simple model of a human (or humaniod robot) that is capable of balancing on its own. We will make several assumptions:
# 
# - The human's motion is limited to a 2D plane (i.e. leaning backwards and forwards).
# - We only have three degrees of freedom: rotation at the ankle, knee, and hip.
# - The two legs do not move independently, but as one body (i.e. left and right thigh are one rigid body).
# - The forces generated by the muscles will be modeled as ideal torques between the adjacent body segments.
# 
# The following diagram shows the model and all of the parameters.

# In[ ]:


from IPython.display import Image


# In[ ]:


Image('figures/human_balance_diagram.png')


# Reference Frames
# ----------------
# 
# There are four reference frames and six important points. The orange inertial reference frame, $I$, is attached to the foot which is rigidly attached to the ground. The blue lower leg reference frame, $L$, is attached to the foot by a pin joint at the ankle point $A$ and rotates relative to the foot through $\theta_1$. The green upper leg reference frame, $U$, is attached to the lower leg by a pin joint at the knee point $K$ and rotates relative to the lower leg through angle $\theta_2$. The red torso reference frame, $T$, is pinned to the upper leg at the hip point, $H$, and rotates relative to the upper leg through the angle $\theta_3$. Note that all rotations are about the $z$ axis and that they are relative to the orientation of the preceding body.
# 
# Geometry
# --------
# 
# The lower and upper legs' lengths are defined as $l_L$ and $l_U$.
# 
# Mass Centers
# -------------
# 
# The three points $L_o$, $U_o$, $T_o$ are the mass centers of the body segments. These are each located on the line connecting the proximal and distal joints of each body segment and are located by the dimensions: $d_L$, $d_U$, and $d_T$.
# 
# Gravity
# -------
# 
# Gravity is directed downwards ($-y$) and applies a force with a magnitude of $m_Lg,m_Ug,m_Tg$ at each mass center, respectively.
# 
# Torques
# -------
# 
# Three torques represent the forces due to muscles contracting. The ankle $T_A$, knee $T_K$, and hip $T_H$ torques apply equal and opposite torques to the adjoining body segments.
# 
#