# PaBiRoboy dynamic equations

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

```python
from __future__ import print_function, division
from sympy import symbols, simplify, Matrix
from sympy import trigsimp
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, Particle, inertia, RigidBody, KanesMethod
from numpy import deg2rad, rad2deg, array, zeros, linspace
from sympy.physics.vector import init_vprinting, vlatex
import numpy as np
from scipy.integrate import odeint
from sympy.utilities.codegen import codegen
from pydy.codegen.ode_function_generators import generate_ode_function
from matplotlib.pyplot import plot, legend, xlabel, ylabel, rcParams
rcParams['figure.figsize'] = (14.0, 6.0)
```

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)
```

## Interial frames

```python
inertial_frame = ReferenceFrame('I')
lower_leg_left_frame = ReferenceFrame('R_1')
upper_leg_left_frame = ReferenceFrame('R_2')
hip_frame = ReferenceFrame('R_3')
upper_leg_right_frame = ReferenceFrame('R_4')
lower_leg_right_frame = ReferenceFrame('R_5')
```

## Angles

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

```python
lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z))
simplify(lower_leg_left_frame.dcm(inertial_frame))
```

$$\left[\begin{matrix}\operatorname{cos}\left(\phi\right) & \operatorname{sin}\left(\phi\right) & 0\\- \operatorname{sin}\left(\phi\right) & \operatorname{cos}\left(\phi\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

```python
upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis', (theta0, -lower_leg_left_frame.z))
simplify(upper_leg_left_frame.dcm(inertial_frame))
```

$$\left[\begin{matrix}\operatorname{cos}\left(\phi - \theta_{0}\right) & \operatorname{sin}\left(\phi - \theta_{0}\right) & 0\\- \operatorname{sin}\left(\phi - \theta_{0}\right) & \operatorname{cos}\left(\phi - \theta_{0}\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

```python
hip_frame.orient(upper_leg_left_frame, 'Axis', (theta1, -upper_leg_left_frame.z))
hip_frame.dcm(inertial_frame)
```

$$\left[\begin{matrix}\left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{sin}\left(\theta_{1}\right) + \operatorname{cos}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right)\right) \operatorname{cos}\left(\phi\right) - \left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right) - \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{0}\right)\right) \operatorname{sin}\left(\phi\right) & \left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{sin}\left(\theta_{1}\right) + \operatorname{cos}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right)\right) \operatorname{sin}\left(\phi\right) + \left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right) - \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{0}\right)\right) \operatorname{cos}\left(\phi\right) & 0\\- \left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{sin}\left(\theta_{1}\right) + \operatorname{cos}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right)\right) \operatorname{sin}\left(\phi\right) + \left(\operatorname{sin}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right) + \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{0}\right)\right) \operatorname{cos}\left(\phi\right) & \left(- \operatorname{sin}\left(\theta_{0}\right) \operatorname{sin}\left(\theta_{1}\right) + \operatorname{cos}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right)\right) \operatorname{cos}\left(\phi\right) + \left(\operatorname{sin}\left(\theta_{0}\right) \operatorname{cos}\left(\theta_{1}\right) + \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{0}\right)\right) \operatorname{sin}\left(\phi\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

```python
upper_leg_right_frame.orient(hip_frame, 'Axis', (theta2, -hip_frame.z))
simplify(upper_leg_right_frame.dcm(inertial_frame))
```

$$\left[\begin{matrix}\operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & - \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & 0\\\operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

```python
lower_leg_right_frame.orient(upper_leg_right_frame, 'Axis', (theta3, -upper_leg_right_frame.z))
simplify(lower_leg_right_frame.dcm(inertial_frame))
```

$$\left[\begin{matrix}\operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & - \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & 0\\\operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

## Points and Locations

```python
origin = Point('Origin')
ankle_left = Point('AnkleLeft')
knee_left = Point('KneeLeft')
hip_left = Point('HipLeft')
hip_center = Point('HipCenter')
hip_right = Point('HipRight')
knee_right = Point('KneeRight')
ankle_right = Point('AnkleRight')
```

```python
lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3')
```

```python
origin = Point('Origin')
ankle_left = Point('AnkleLeft')
knee_left = Point('KneeLeft')
hip_left = Point('HipLeft')
hip_center = Point('HipCenter')
hip_right = Point('HipRight')
knee_right = Point('KneeRight')
ankle_right = Point('AnkleRight')

# here go the lengths of your robots links
lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3')

ankle_left.set_pos(origin, (0 * inertial_frame.y)+(0 * inertial_frame.x))
#ankle_left.pos_from(origin).express(inertial_frame).simplify()

knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y)
#knee_left.pos_from(origin).express(inertial_frame).simplify()

hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y)
#hip_left.pos_from(origin).express(inertial_frame).simplify()

hip_center.set_pos(hip_left, hip_length/2 * hip_frame.x)
#hip_center.pos_from(origin).express(inertial_frame).simplify()

hip_right.set_pos(hip_center, hip_length/2 * hip_frame.x)
#hip_right.pos_from(origin).express(inertial_frame).simplify()

knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)
#knee_right.pos_from(origin).express(inertial_frame).simplify()

ankle_right.set_pos(knee_right, lower_leg_length * -lower_leg_right_frame.y)
#ankle_right.pos_from(origin).express(inertial_frame).simplify()

```

```python
lower_leg_left_com_length = lower_leg_length/2
upper_leg_left_com_length = upper_leg_length/2
hip_com_length = hip_length/2
upper_leg_right_com_length = upper_leg_length/2
lower_leg_right_com_length = lower_leg_length/2

lower_leg_left_mass_center = Point('L_COMleft')
upper_leg_left_mass_center = Point('U_COMleft')
hip_mass_center = Point('H_COMleft')
upper_leg_right_mass_center = Point('U_COMright')
lower_leg_right_mass_center = Point('L_COMright')
```

```python
lower_leg_left_mass_center.set_pos(ankle_left, lower_leg_left_com_length * lower_leg_left_frame.y)
#lower_leg_left_mass_center.pos_from(origin).express(inertial_frame).simplify()

upper_leg_left_mass_center.set_pos(knee_left, upper_leg_left_com_length * upper_leg_left_frame.y)
#upper_leg_left_mass_center.pos_from(origin).express(inertial_frame).simplify()

hip_mass_center.set_pos(hip_center, 0 * hip_frame.x)
#hip_mass_center.pos_from(origin).express(inertial_frame).simplify()

upper_leg_right_mass_center.set_pos(knee_right, upper_leg_right_com_length * upper_leg_right_frame.y)
#upper_leg_right_mass_center.pos_from(origin).express(inertial_frame).simplify()

lower_leg_right_mass_center.set_pos(ankle_right, lower_leg_right_com_length * lower_leg_right_frame.y)
lower_leg_right_mass_center.pos_from(origin).express(inertial_frame).simplify()
```

$$(- \frac{l_{1}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right))\mathbf{\hat{i}_x} + (- \frac{l_{1}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} \operatorname{cos}\left(\phi\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right))\mathbf{\hat{i}_y}$$

## Kinematical Differential Equations

```python
omega0, omega1, omega2, omega3, psi = dynamicsymbols('omega0, omega1, omega2, omega3, psi')
kinematical_differential_equations = [omega0 - theta0.diff(),
                                      omega1 - theta1.diff(),
                                      omega2 - theta2.diff(),
                                      omega3 - theta3.diff(),
                                        psi - phi.diff(),
                                        ]

kinematical_differential_equations
```

$$\left [ \omega_{0} - \dot{\theta}_{0}, \quad \omega_{1} - \dot{\theta}_{1}, \quad \omega_{2} - \dot{\theta}_{2}, \quad \omega_{3} - \dot{\theta}_{3}, \quad \psi - \dot{\phi}\right ]$$

## Angular Velocities

```python
lower_leg_left_frame.set_ang_vel(inertial_frame,         0 * inertial_frame.z)
#lower_leg_left_frame.ang_vel_in(inertial_frame)

upper_leg_left_frame.set_ang_vel(upper_leg_left_frame,   omega0     * inertial_frame.z)
#upper_leg_left_frame.ang_vel_in(inertial_frame)

hip_frame.set_ang_vel(hip_frame,   omega1     * inertial_frame.z)
#hip_frame.ang_vel_in(inertial_frame)

upper_leg_right_frame.set_ang_vel(upper_leg_right_frame, omega2     * inertial_frame.z)
#upper_leg_right_frame.ang_vel_in(inertial_frame)

lower_leg_right_frame.set_ang_vel(lower_leg_right_frame, omega3     * inertial_frame.z)
lower_leg_right_frame.ang_vel_in(inertial_frame)

```

$$-  \dot{\theta}_{3}\mathbf{\hat{r_4}_z} -  \dot{\theta}_{2}\mathbf{\hat{r_3}_z} -  \dot{\theta}_{1}\mathbf{\hat{r_2}_z} -  \dot{\theta}_{0}\mathbf{\hat{r_1}_z}$$

# Linear Velocities

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

```python
ankle_left.set_vel(inertial_frame, 0)
origin.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
knee_left.v2pt_theory(ankle_left, inertial_frame, lower_leg_left_frame)
#knee_left.vel(inertial_frame)

hip_left.v2pt_theory(knee_left, inertial_frame, upper_leg_left_frame)
#hip_left.vel(inertial_frame)

hip_center.v2pt_theory(hip_left, inertial_frame, hip_frame)
#hip_center.vel(inertial_frame)

hip_mass_center.v2pt_theory(hip_center, inertial_frame, hip_frame)
#hip_mass_center.vel(inertial_frame)

hip_right.v2pt_theory(hip_center, inertial_frame, hip_frame)
#hip_right.vel(inertial_frame)

knee_right.v2pt_theory(hip_right, inertial_frame, upper_leg_right_frame)
#knee_right.vel(inertial_frame)

ankle_right.v2pt_theory(knee_right, inertial_frame, lower_leg_right_frame)
#ankle_right.vel(inertial_frame)

lower_leg_left_mass_center.v2pt_theory(ankle_left, inertial_frame, lower_leg_left_frame)
#lower_leg_left_mass_center.vel(inertial_frame)

lower_leg_right_mass_center.v2pt_theory(ankle_right, inertial_frame, lower_leg_right_frame)
#lower_leg_right_mass_center.vel(inertial_frame)

upper_leg_left_mass_center.v2pt_theory(knee_left, inertial_frame, upper_leg_left_frame)
#upper_leg_left_mass_center.vel(inertial_frame)

upper_leg_right_mass_center.v2pt_theory(knee_right, inertial_frame, upper_leg_right_frame)
upper_leg_right_mass_center.vel(inertial_frame)
```

$$l_{2} \dot{\theta}_{0}\mathbf{\hat{r_2}_x} + l_{3} \left(- \dot{\theta}_{0} - \dot{\theta}_{1}\right)\mathbf{\hat{r_3}_y} + \frac{l_{2}}{2} \left(- \dot{\theta}_{0} - \dot{\theta}_{1} - \dot{\theta}_{2}\right)\mathbf{\hat{r_4}_x}$$

# Masses, Inertia, Rigid Bodies

```python
lower_leg_mass, upper_leg_mass, hip_mass = symbols('m_L, m_U, m_H')

lower_leg_inertia, upper_leg_inertia, hip_inertia = symbols('I_Lz, I_Uz, I_Hz')

lower_leg_left_inertia_dyadic = inertia(lower_leg_left_frame, lower_leg_inertia, lower_leg_inertia, lower_leg_inertia)
lower_leg_left_central_inertia = (lower_leg_left_inertia_dyadic, lower_leg_left_mass_center)
lower_leg_left_inertia_dyadic.to_matrix(lower_leg_left_frame)

upper_leg_left_inertia_dyadic = inertia(upper_leg_left_frame, upper_leg_inertia, upper_leg_inertia, upper_leg_inertia)
upper_leg_left_central_inertia = (upper_leg_left_inertia_dyadic, upper_leg_left_mass_center)
upper_leg_left_inertia_dyadic.to_matrix(upper_leg_left_frame)

hip_inertia_dyadic = inertia(hip_frame, hip_inertia, hip_inertia, hip_inertia)
hip_central_inertia = (hip_inertia_dyadic, hip_mass_center)
hip_inertia_dyadic.to_matrix(hip_frame)

upper_leg_right_inertia_dyadic = inertia(upper_leg_right_frame, upper_leg_inertia, upper_leg_inertia, upper_leg_inertia)
upper_leg_right_central_inertia = (upper_leg_right_inertia_dyadic, upper_leg_right_mass_center)
upper_leg_right_inertia_dyadic.to_matrix(upper_leg_right_frame)

lower_leg_right_inertia_dyadic = inertia(lower_leg_right_frame, lower_leg_inertia, lower_leg_inertia, lower_leg_inertia)
lower_leg_right_central_inertia = (lower_leg_right_inertia_dyadic, lower_leg_right_mass_center)
lower_leg_right_inertia_dyadic.to_matrix(lower_leg_right_frame)

lower_leg_left = RigidBody('Lower Leg Left', lower_leg_left_mass_center, lower_leg_left_frame, lower_leg_mass, lower_leg_left_central_inertia)

upper_leg_left = RigidBody('Upper Leg Left', upper_leg_left_mass_center, upper_leg_left_frame, upper_leg_mass, upper_leg_left_central_inertia)

hip = RigidBody('Hip', hip_mass_center, hip_frame, hip_mass, hip_central_inertia)

upper_leg_right = RigidBody('Upper Leg Right', upper_leg_right_mass_center, upper_leg_right_frame, upper_leg_mass, upper_leg_right_central_inertia)

lower_leg_right = RigidBody('Lower Leg Right', lower_leg_right_mass_center, lower_leg_right_frame, lower_leg_mass, lower_leg_right_central_inertia)

particles = []
particles.append(Particle('ankle_left', ankle_left, 0))
particles.append(Particle('knee_left', knee_left, 0))
particles.append(Particle('hip_left', hip_left, 0))
particles.append(Particle('hip_center', hip_center, 0))
particles.append(Particle('hip_right', hip_right, 0))
particles.append(Particle('knee_right', knee_right, 0))
particles.append(Particle('ankle_right', ankle_right, 0))
particles

mass_centers = []
mass_centers.append(Particle('lower_leg_left_mass_center', lower_leg_left_mass_center, lower_leg_mass))
mass_centers.append(Particle('upper_leg_left_mass_center', upper_leg_left_mass_center, upper_leg_mass))
mass_centers.append(Particle('hip_mass_center', hip_mass_center, hip_mass))
mass_centers.append(Particle('hip_mass_center', hip_mass_center, hip_mass))
mass_centers.append(Particle('hip_mass_center', hip_mass_center, hip_mass))
mass_centers.append(Particle('lower_leg_left_mass_center', lower_leg_right_mass_center, lower_leg_mass))
mass_centers.append(Particle('upper_leg_left_mass_center', upper_leg_right_mass_center, upper_leg_mass))
mass_centers
```

    [lower_leg_left_mass_center,
     upper_leg_left_mass_center,
     hip_mass_center,
     hip_mass_center,
     hip_mass_center,
     lower_leg_left_mass_center,
     upper_leg_left_mass_center]

# Forces and Torques

```python
g = symbols('g')
```

```python
lower_leg_left_grav_force = (lower_leg_left_mass_center, -lower_leg_mass * g * inertial_frame.y)
upper_leg_left_grav_force = (upper_leg_left_mass_center, -upper_leg_mass * g * inertial_frame.y)
hip_grav_force = (hip_mass_center, -hip_mass * g * inertial_frame.y)
upper_leg_right_grav_force = (upper_leg_right_mass_center, -upper_leg_mass * g * inertial_frame.y)
lower_leg_right_grav_force = (lower_leg_right_mass_center, -lower_leg_mass * g * inertial_frame.y)

ankle_torque0, knee_torque0, hip_torque0, hip_torque1, knee_torque1, ankle_torque1 = dynamicsymbols('T_a0, T_k0, T_h0, T_h1, T_k1, T_a1')

lower_leg_left_torque_vector = ankle_torque0 * inertial_frame.z - knee_torque0 * inertial_frame.z
upper_leg_left_torque_vector = knee_torque0 * inertial_frame.z - hip_torque0 * inertial_frame.z
hip_left_torque_vector = hip_torque0 * inertial_frame.z - hip_torque1 * inertial_frame.z
hip_right_torque_vector = hip_torque1 * inertial_frame.z - knee_torque1 * inertial_frame.z
upper_leg_right_torque_vector = knee_torque1 * inertial_frame.z - ankle_torque1 * inertial_frame.z
lower_leg_right_torque_vector = ankle_torque1 * inertial_frame.z

lower_leg_left_torque = (lower_leg_left_frame, lower_leg_left_torque_vector)
upper_leg_left_torque = (upper_leg_left_frame, upper_leg_left_torque_vector)
hip_left_torque = (hip_frame, hip_left_torque_vector)
hip_right_torque = (hip_frame, hip_right_torque_vector)
upper_leg_right_torque = (upper_leg_right_frame, upper_leg_right_torque_vector)
lower_leg_right_torque = (lower_leg_right_frame, lower_leg_right_torque_vector)
```

# Equations of Motion

```python
coordinates = [theta0, theta1, theta2, theta3, phi]
coordinates
```

$$\left [ \theta_{0}, \quad \theta_{1}, \quad \theta_{2}, \quad \theta_{3}, \quad \phi\right ]$$

```python
speeds = [omega0, omega1, omega2, omega3, psi]
speeds
```

$$\left [ \omega_{0}, \quad \omega_{1}, \quad \omega_{2}, \quad \omega_{3}, \quad \psi\right ]$$

```python
kinematical_differential_equations
```

$$\left [ \omega_{0} - \dot{\theta}_{0}, \quad \omega_{1} - \dot{\theta}_{1}, \quad \omega_{2} - \dot{\theta}_{2}, \quad \omega_{3} - \dot{\theta}_{3}, \quad \psi - \dot{\phi}\right ]$$

```python
kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations)
```

```python
loads = [lower_leg_left_grav_force,
         upper_leg_left_grav_force,
         hip_grav_force,
         upper_leg_right_grav_force,
         lower_leg_right_grav_force,
         lower_leg_left_torque,
         upper_leg_left_torque,
         hip_left_torque,
         hip_right_torque,
         upper_leg_right_torque,
         lower_leg_right_torque]
loads
```

    [(L_COMleft, - g*m_L*I.y),
     (U_COMleft, - g*m_U*I.y),
     (H_COMleft, - g*m_H*I.y),
     (U_COMright, - g*m_U*I.y),
     (L_COMright, - g*m_L*I.y),
     (R_1, (T_a0 - T_k0)*I.z),
     (R_2, (-T_h0 + T_k0)*I.z),
     (R_3, (T_h0 - T_h1)*I.z),
     (R_3, (T_h1 - T_k1)*I.z),
     (R_4, (-T_a1 + T_k1)*I.z),
     (R_5, T_a1*I.z)]

```python
bodies = [lower_leg_left, upper_leg_left, hip, upper_leg_right, lower_leg_right]
bodies
```

    [Lower Leg Left, Upper Leg Left, Hip, Upper Leg Right, Lower Leg Right]

```python
fr, frstar = kane.kanes_equations(loads, bodies)
```

```python
trigsimp(fr + frstar)
```

$$\left[\begin{matrix}- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{H} \operatorname{sin}\left(\phi - \theta_{0}\right) - g l_{2} m_{L} \operatorname{sin}\left(\phi - \theta_{0}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{3 g}{2} l_{2} m_{U} \operatorname{sin}\left(\phi - \theta_{0}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + \frac{g l_{3}}{2} m_{H} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{L} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{U} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - \frac{l_{2}^{2} m_{U}}{2} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{H} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{H} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{L} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) + l_{2} l_{3} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{U} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} - 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right)\right) \dot{\omega}_{3} - \left(I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(- 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right)\right) \dot{\omega}_{2} - \left(I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3} m_{H}}{4} \left(- 2 l_{2} \operatorname{sin}\left(\theta_{1}\right) + l_{3}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- \frac{l_{2}^{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right)\right) \dot{\omega}_{1} - \left(I_{Hz} + I_{Lz} + 2 I_{Uz} + \frac{l_{2}^{2} m_{U}}{4} + m_{H} \left(l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) + \frac{l_{3}^{2}}{4}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - l_{1} l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - 2 l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + 2 l_{2}^{2} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{5 l_{2}^{2}}{4} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right)\right) \dot{\omega}_{0} - T_{k0}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + \frac{g l_{3}}{2} m_{H} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{L} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{U} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - \frac{l_{2} l_{3}}{2} m_{H} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{L} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{U} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right)\right) \dot{\omega}_{3} - \left(I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right)\right) \dot{\omega}_{2} - \left(I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3}^{2} m_{H}}{4} + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(\frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right)\right) \dot{\omega}_{1} - \left(I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3} m_{H}}{4} \left(- 2 l_{2} \operatorname{sin}\left(\theta_{1}\right) + l_{3}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- \frac{l_{2}^{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right)\right) \dot{\omega}_{0} - T_{h0}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right)\right)\right) \dot{\omega}_{3} - \left(I_{Lz} + I_{Uz} + \frac{l_{2}^{2} m_{U}}{4} + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) + l_{2}^{2}\right)\right) \dot{\omega}_{2} - \left(I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right)\right) \dot{\omega}_{1} - \left(I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(- 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right)\right) \dot{\omega}_{0} - T_{k1}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) - \left(I_{Lz} + \frac{l_{1}^{2} m_{L}}{4}\right) \dot{\omega}_{3} - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right)\right)\right) \dot{\omega}_{2} - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right)\right) \dot{\omega}_{1} - \left(I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} - 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right)\right) \dot{\omega}_{0} - T_{a1}\\0\end{matrix}\right]$$

Keep in mind that out utlimate goal is to have the equations of motion in first order form:

$$ \dot{\mathbf{x}} = \mathbf{g}(\mathbf{x}, t) $$

The equations of motion are linear in terms of the derivatives of the generalized speeds and the `KanesMethod` class automatically puts the equations in a more useful form for the next step of numerical simulation:

$$ \mathbf{M}(\mathbf{x}, t)\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, t) $$

Note that

$$ \mathbf{g} = \mathbf{M}^{-1}(\mathbf{x}, t) \mathbf{f}(\mathbf{x}, t) $$

and that $\mathbf{g}$ can be computed analytically but for non-toy problems, it is best to do this numerically. So we will simply generate the $\mathbf{M}$ and $\mathbf{f}$ matrices for later use.

The mass matrix, $\mathbf{M}$, can be accessed with the `mass_matrix` method (use `mass_matrix_full` to include the kinematical differential equations too. We can use `trigsimp` again to make this relatively compact:

```python
mass_matrix = trigsimp(kane.mass_matrix_full)
mass_matrix
```

$$\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & I_{Hz} + I_{Lz} + 2 I_{Uz} + \frac{l_{2}^{2} m_{U}}{4} + m_{H} \left(l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) + \frac{l_{3}^{2}}{4}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - l_{1} l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - 2 l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + 2 l_{2}^{2} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{5 l_{2}^{2}}{4} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) & I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3} m_{H}}{4} \left(- 2 l_{2} \operatorname{sin}\left(\theta_{1}\right) + l_{3}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- \frac{l_{2}^{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) & I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(- 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} - 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right) & 0\\0 & 0 & 0 & 0 & 0 & I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3} m_{H}}{4} \left(- 2 l_{2} \operatorname{sin}\left(\theta_{1}\right) + l_{3}\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(- \frac{l_{2}^{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{1}\right) - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) & I_{Hz} + I_{Lz} + I_{Uz} + \frac{l_{3}^{2} m_{H}}{4} + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - l_{1} l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - 2 l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) + m_{U} \left(\frac{l_{2}^{2}}{4} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right) + l_{3}^{2}\right) & I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right) & 0\\0 & 0 & 0 & 0 & 0 & I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(- 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} - \frac{l_{1} l_{2}}{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) & I_{Lz} + I_{Uz} + \frac{l_{2} m_{U}}{4} \left(l_{2} - 2 l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) - \frac{l_{1} l_{3}}{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} - l_{2} l_{3} \operatorname{sin}\left(\theta_{2}\right)\right) & I_{Lz} + I_{Uz} + \frac{l_{2}^{2} m_{U}}{4} + m_{L} \left(\frac{l_{1}^{2}}{4} + l_{1} l_{2} \operatorname{cos}\left(\theta_{3}\right) + l_{2}^{2}\right) & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right)\right) & 0\\0 & 0 & 0 & 0 & 0 & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} - 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right) & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right) - 2 l_{3} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\right) & I_{Lz} + \frac{l_{1} m_{L}}{4} \left(l_{1} + 2 l_{2} \operatorname{cos}\left(\theta_{3}\right)\right) & I_{Lz} + \frac{l_{1}^{2} m_{L}}{4} & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]$$

The right hand side, $\mathbf{f}$, is a vector function of all the non-inertial forces (gyroscopic, external, coriolis, etc):

```python
forcing_vector = trigsimp(kane.forcing_full)
forcing_vector
```

$$\left[\begin{matrix}\omega_{0}\\\omega_{1}\\\omega_{2}\\\omega_{3}\\\psi\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{H} \operatorname{sin}\left(\phi - \theta_{0}\right) - g l_{2} m_{L} \operatorname{sin}\left(\phi - \theta_{0}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{3 g}{2} l_{2} m_{U} \operatorname{sin}\left(\phi - \theta_{0}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + \frac{g l_{3}}{2} m_{H} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{L} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{U} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) - l_{2}^{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - \frac{l_{2}^{2} m_{U}}{2} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{H} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{H} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{L} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) + l_{2} l_{3} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{U} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - T_{k0}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + \frac{g l_{3}}{2} m_{H} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{L} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) + g l_{3} m_{U} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - \frac{l_{2} l_{3}}{2} m_{H} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{L} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) + \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - l_{2} l_{3} m_{U} \omega^{2}_{0} \operatorname{cos}\left(\theta_{1}\right) - T_{h0}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - g l_{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{g l_{2}}{2} m_{U} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2} - \omega_{3}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) + l_{2}^{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \frac{l_{2}^{2} m_{U}}{2} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) - l_{2} l_{3} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - \frac{l_{2} l_{3}}{2} m_{U} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2}\right) - T_{k1}\\- \frac{g l_{1}}{2} m_{L} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{2}}{2} m_{L} \left(- \omega_{0} - \omega_{1} - \omega_{2}\right)^{2} \operatorname{sin}\left(\theta_{3}\right) + \frac{l_{1} l_{2}}{2} m_{L} \omega^{2}_{0} \operatorname{sin}\left(\theta_{1} + \theta_{2} + \theta_{3}\right) - \frac{l_{1} l_{3}}{2} m_{L} \left(- \omega_{0} - \omega_{1}\right)^{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) - T_{a1}\\0\end{matrix}\right]$$

# Simulation

```python
from scipy.integrate import odeint
from sympy.utilities.codegen import codegen
from pydy.codegen.ode_function_generators import generate_ode_function
from matplotlib.pyplot import plot, legend, xlabel, ylabel, rcParams
rcParams['figure.figsize'] = (14.0, 6.0)

specified = [ankle_torque0, knee_torque0, hip_torque0, hip_torque1, knee_torque1, ankle_torque1]
numerical_specified = zeros(6)

x0 = zeros(10)
x0

x0[0] = deg2rad(80)
x0[1] = -deg2rad(80)
x0[2] = -deg2rad(80)
x0[3] = deg2rad(80)
x0[4] = 0

x0
```

    array([ 1.3962634, -1.3962634, -1.3962634,  1.3962634,  0.       ,
            0.       ,  0.       ,  0.       ,  0.       ,  0.       ])

```python
#%% Jacobian for the right ankle, which we will try to leave where it is when
#   moving the hip center
print ('calculating jacobian for the right ankle')
F0 = ankle_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
F0 = Matrix([F0[0], F0[1]])
F0
```

    calculating jacobian for the right ankle

$$\left[\begin{matrix}- l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\\- l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} \operatorname{cos}\left(\phi\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\end{matrix}\right]$$

```python
J_ankleRight = F0.jacobian([theta0, theta1, theta2, theta3, phi])
J_ankleRight
```

$$\left[\begin{matrix}- l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right) & - l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right) & - l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & - l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{1} \operatorname{cos}\left(\phi\right) - l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) + l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\\l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) + l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) & l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) & l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) & l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) & - l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\end{matrix}\right]$$

```python
F1 = hip_center.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
F1 = Matrix([F1[0], F1[1]])
F1
```

$$\left[\begin{matrix}- l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) + \frac{l_{3}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\\l_{1} \operatorname{cos}\left(\phi\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - \frac{l_{3}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\end{matrix}\right]$$

```python
J_hipCenter = F1.jacobian([theta0, theta1, theta2, theta3, phi])
J_hipCenter
```

$$\left[\begin{matrix}l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - \frac{l_{3}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right) & - \frac{l_{3}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right) & 0 & 0 & - l_{1} \operatorname{cos}\left(\phi\right) - l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) + \frac{l_{3}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\\l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) - \frac{l_{3}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) & - \frac{l_{3}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right) & 0 & 0 & - l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) + \frac{l_{3}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\end{matrix}\right]$$

```python
#%% we stack the two jacobians
J = J_ankleRight.col_join(J_hipCenter)
J.shape
```

$$\left ( 4, \quad 5\right )$$

```python
#%% lets try the pseudo inverse with a couple of real values
values = {lower_leg_length: 0.4, upper_leg_length: 0.54, hip_length: 0.2, theta0: x0[0], theta1: x0[1], theta2: x0[2], theta3: x0[3], phi: x0[4]}
Jpinv = J.subs(values).evalf().pinv()
Jpinv
```

$$\left[\begin{matrix}-0.0459332479170267 & 0.219478074655015 & -2.3994104929455 & -2.31969900118024\\0.235092856483776 & -1.12331981399207 & -0.514831317577474 & 1.87254742697749\\-0.235092856483778 & -0.757099837648197 & 0.514831317577473 & 1.88829187630304\\-2.45406675208297 & 2.10175902875642 & 2.39941049294551 & -2.32277520564262\\-0.00872301122043817 & 0.0416802598264825 & -2.48089742314152 & -0.440525356532326\end{matrix}\right]$$

```python
#%% we stack the two endeffektor points, which we will evaluate in the integration
F2 = F0.col_join(F1)
F2
```

$$\left[\begin{matrix}- l_{1} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) - l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) + l_{3} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\\- l_{1} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2} + \theta_{3}\right) + l_{1} \operatorname{cos}\left(\phi\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - l_{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1} + \theta_{2}\right) - l_{3} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\\- l_{1} \operatorname{sin}\left(\phi\right) - l_{2} \operatorname{sin}\left(\phi - \theta_{0}\right) + \frac{l_{3}}{2} \operatorname{cos}\left(- \phi + \theta_{0} + \theta_{1}\right)\\l_{1} \operatorname{cos}\left(\phi\right) + l_{2} \operatorname{cos}\left(\phi - \theta_{0}\right) - \frac{l_{3}}{2} \operatorname{sin}\left(- \phi + \theta_{0} + \theta_{1}\right)\end{matrix}\right]$$

```python
#%% this defines how long you want to simulate, we simulate for one second with 30 fps
#   simulation can take quite a while...
frames_per_sec = 30
final_time = 1

t = linspace(0.0, final_time, final_time * frames_per_sec)
```

```python
numerical_constants = array([0.42,  # lower_leg_length [m]
                             0.54, # upper_leg_length [m]
                             0.2, # hip_length

                             1.0,  # lower_leg_mass [kg]
                             1.5,  # upper_leg_mass [kg]
                             2.0,  # hip_mass [kg]

                             0.1,  # lower_leg_inertia [kg*m^2]
                             0.2,  # upper_leg_inertia [kg*m^2]
                             0.1,  # hip_inertia [kg*m^2]

                             9.81],  # acceleration due to gravity [m/s^2]
                            )
numerical_constants
```

    array([ 0.42,  0.54,  0.2 ,  1.  ,  1.5 ,  2.  ,  0.1 ,  0.2 ,  0.1 ,  9.81])

```python
constants = [lower_leg_length,
             upper_leg_length,
             hip_length,

             lower_leg_mass,
             upper_leg_mass,
             hip_mass,

             lower_leg_inertia,
             upper_leg_inertia,
             hip_inertia,

             g]
constants
```

$$\left [ l_{1}, \quad l_{2}, \quad l_{3}, \quad m_{L}, \quad m_{U}, \quad m_{H}, \quad I_{Lz}, \quad I_{Uz}, \quad I_{Hz}, \quad g\right ]$$

```python
#%%
x0 = zeros(10)
x0

x0[0] = deg2rad(80)
x0[1] = -deg2rad(80)
x0[2] = -deg2rad(80)
x0[3] = deg2rad(80)
x0[4] = 0

x0
```

    array([ 1.3962634, -1.3962634, -1.3962634,  1.3962634,  0.       ,
            0.       ,  0.       ,  0.       ,  0.       ,  0.       ])

```python

#%% Use this for full kinematics simulation
#right_hand_side = generate_ode_function(forcing_vector, coordinates, speeds,
#                                        constants, mass_matrix=mass_matrix,
#                                        specifieds=specified, generator='cython')
#
#args = {'constants': numerical_constants,
#        'specified': numerical_specified}
#
#right_hand_side(x0, 0.0, numerical_specified, numerical_constants)
##%%
#y = odeint(right_hand_side, x0, t, args=(numerical_specified, numerical_constants))
#y

#%% only useful when you want to simulate full kinematics, use in right_hand_side equation
#from sympy import lambdify, solve
#M_func = lambdify(coordinates + speeds + constants + specified, mass_matrix) # Create a callable function to evaluate the mass matrix
#f_func = lambdify(coordinates + speeds + constants + specified, forcing_vector)     # Create a callable function to evaluate the forcing vector
#%% inverse kinematics gains, you can balance here how much the respective
#   targets should be tried to reach
kp = np.eye(4)
kp[0,0] = 10
kp[1,1] = 10
kp[2,2] = 10
kp[3,3] = 10
kp
#%%
kpN = np.zeros((4,4),dtype=float)
kpN[1,1] = 10
kpN
#%% These are our target points
L = 1 # this defines how far apart the left and right ankle should be, y=0 so the
      # foot does not lift of the ground
      # the last two values are x and y coordinates for the hip center
x_des = Matrix([L,0,0.7,0.1])
x_des.shape
#%% This is the intergration function (the inverse kinematics)
#   I highly recommend you have a look at this textbook http://smpp.northwestern.edu/savedLiterature/Spong_Textbook.pdf
#   especially chapter 5.10 Inverse Velocity and Acceleration
i = 0
def right_hand_side(x, t, args):
    """Returns the derivatives of the states.

    Parameters
    ----------
    x : ndarray, shape(2 * (n + 1))
        The current state vector.
    t : float
        The current time.
    args : ndarray
        The constants.

    Returns
    -------
    dx : ndarray, shape(2 * (n + 1))
        The derivative of the state.

    """
    global i
    r = 0.0                                  # The input force is always zero
    arguments = np.hstack((x, args))      # States, input, and parameters
    values = {lower_leg_length: 0.4, upper_leg_length: 0.54, hip_length: 0.2,
              theta0: x[0], theta1: x[1], theta2: x[2], theta3: x[3], phi: x[4],
                omega0: 0, omega1: 0, omega2: 0, omega3: 0, psi: 0}
    Jpinv = J.subs(values).evalf().pinv()
    # use this for nullspace movements
#    N=np.eye(4)-Jpinv*J.subs(values).evalf()
    x_current = F2.subs(values).evalf()                                         # use this for nullspace movements
    dq = np.array(Jpinv*(kp*( x_des - x_current ))).astype(float).T[0]# + N*(kpN*(Matrix([0,-deg2rad(80),0,0,0])-Matrix([x[0],x[1],x[2],x[3])))).astype(float).T[0]
    dq = np.hstack((dq,np.zeros(5,dtype=float)))
    if i%10==0:
        print(x_current)
    i = i+1
    return dq
    # use this for full kinematic simulation
#    dq = np.array(np.linalg.solve(M_func(*arguments),  # Solving for the derivatives
#                  f_func(*arguments))).T[0]
#    return dq
```

```python
#%% Ok lets simulate
print('integrating')
args = (np.hstack((numerical_constants,)),)
y = odeint(right_hand_side, x0, t, args)#,full_output = 1,mxstep=1000000
#y = np.hstack((y,np.zeros(y.shape)))
print(y)
print('done integrating')
```

    integrating
    Matrix([[1.26359237325318], [0], [0.631796186626592], [0.493770015940142]])
    Matrix([[1.26175159570018], [6.37010906447567e-9], [0.632272477736748], [0.491020164371748]])
    Matrix([[1.25528230436820], [-8.25783376997690e-9], [0.633946400654344], [0.481355924543045]])
    Matrix([[1.24204513938050], [-4.32124976079695e-9], [0.637371479067839], [0.461581463804779]])
    Matrix([[1.22016985269966], [-7.65121452371391e-9], [0.643031651745630], [0.428902850571606]])
    Matrix([[1.19347469825923], [-4.40686484179387e-9], [0.649938948994721], [0.389024040678037]])
    Matrix([[1.16441873966249], [-1.32369283719833e-9], [0.657457097886850], [0.345618520687556]])
    Matrix([[1.13086159526863], [3.19119596654989e-9], [0.666139915607050], [0.295488862780702]])
    Matrix([[1.09762332427629], [8.60721838030418e-9], [0.674740225069592], [0.245835558050327]])
    Matrix([[1.06845152046108], [1.41858201431304e-8], [0.682288346687779], [0.202256984915244]])
    Matrix([[1.04481151791494], [1.23655330506317e-8], [0.688405133486092], [0.166942137274830]])
    Matrix([[1.02579277469106], [2.67137921592525e-9], [0.693326187989628], [0.138530789666744]])
    Matrix([[1.01358973421518], [-1.82801794187948e-9], [0.696483696012692], [0.120301150837070]])
    Matrix([[1.00627090789950], [-6.78787654556645e-10], [0.698377421556476], [0.109367853168821]])
    Matrix([[1.00242339361369], [-1.00974011790766e-9], [0.699372955849698], [0.103620206981665]])
    Matrix([[1.00085360468088], [-8.53671905734488e-10], [0.699779133163587], [0.101275165217524]])
    Matrix([[1.00024778277731], [2.14709708346028e-10], [0.699935886932838], [0.100370152121145]])
    Matrix([[1.00006920332609], [4.67516945296120e-11], [0.699982093856457], [0.100103379898465]])
    Matrix([[1.00001656522374], [-1.12818834135942e-11], [0.699995713807771], [0.100024746112718]])
    [[ 1.3962634  -1.3962634  -1.3962634   1.3962634   0.          0.          0.
       0.          0.          0.        ]
     [ 1.58082794 -1.62016678 -1.57321165  1.84765989 -0.03033541  0.          0.
       0.          0.          0.        ]
     [ 1.67079324 -1.76233809 -1.68716838  2.12830504 -0.088468    0.          0.
       0.          0.          0.        ]
     [ 1.71531961 -1.85578284 -1.76218806  2.31695656 -0.14682271  0.          0.
       0.          0.          0.        ]
     [ 1.73730152 -1.91741631 -1.81150631  2.44799534 -0.19618586  0.          0.
       0.          0.          0.        ]
     [ 1.74806591 -1.9579694  -1.84371585  2.54052487 -0.23486065  0.          0.
       0.          0.          0.        ]
     [ 1.75326896 -1.98459453 -1.86464227  2.60641834 -0.26395708  0.          0.
       0.          0.          0.        ]
     [ 1.75573295 -2.00208225 -1.87822052  2.65352561 -0.28534559  0.          0.
       0.          0.          0.        ]
     [ 1.75686168 -2.01361142 -1.8870601   2.68723902 -0.30085218  0.          0.
       0.          0.          0.        ]
     [ 1.7573498  -2.02126402 -1.89285726  2.71135407 -0.31199988  0.          0.
       0.          0.          0.        ]
     [ 1.75753831 -2.0263886  -1.89669765  2.7285809  -0.31997206  0.          0.
       0.          0.          0.        ]
     [ 1.75759247 -2.02985352 -1.8992705   2.74086793 -0.32565456  0.          0.
       0.          0.          0.        ]
     [ 1.75759082 -2.0322183  -1.90101329  2.74961855 -0.32969648  0.          0.
       0.          0.          0.        ]
     [ 1.75756949 -2.03384574 -1.90220558  2.75584256 -0.33256757  0.          0.
       0.          0.          0.        ]
     [ 1.75754423 -2.03497359 -1.90302811  2.76026484 -0.33460516  0.          0.
       0.          0.          0.        ]
     [ 1.75752123 -2.0357596  -1.90359937  2.7634044  -0.33605036  0.          0.
       0.          0.          0.        ]
     [ 1.75750238 -2.03630975 -1.90399819  2.76563194 -0.33707498  0.          0.
       0.          0.          0.        ]
     [ 1.75748773 -2.03669608 -1.90427773  2.76721166 -0.33780123  0.          0.
       0.          0.          0.        ]
     [ 1.75747671 -2.03696802 -1.90447424  2.7683316  -0.33831589  0.          0.
       0.          0.          0.        ]
     [ 1.75746858 -2.03715978 -1.90461268  2.76912538 -0.33868056  0.          0.
       0.          0.          0.        ]
     [ 1.75746266 -2.03729517 -1.90471035  2.76968789 -0.33893892  0.          0.
       0.          0.          0.        ]
     [ 1.75745838 -2.03739086 -1.90477935  2.77008646 -0.33912196  0.          0.
       0.          0.          0.        ]
     [ 1.75745531 -2.03745853 -1.90482812  2.77036885 -0.33925163  0.          0.
       0.          0.          0.        ]
     [ 1.75745311 -2.03750641 -1.90486262  2.77056891 -0.33934349  0.          0.
       0.          0.          0.        ]
     [ 1.75745155 -2.03754029 -1.90488704  2.77071064 -0.33940856  0.          0.
       0.          0.          0.        ]
     [ 1.75745043 -2.03756428 -1.90490432  2.77081103 -0.33945465  0.          0.
       0.          0.          0.        ]
     [ 1.75744964 -2.03758126 -1.90491655  2.77088215 -0.3394873   0.          0.
       0.          0.          0.        ]
     [ 1.75744908 -2.03759329 -1.90492521  2.77093253 -0.33951043  0.          0.
       0.          0.          0.        ]
     [ 1.75744868 -2.03760181 -1.90493135  2.77096822 -0.33952681  0.          0.
       0.          0.          0.        ]
     [ 1.7574484  -2.03760784 -1.90493569  2.77099349 -0.33953841  0.          0.
       0.          0.          0.        ]]
    done integrating

```python
#%% Plot
# here we plot a little
print("Plotting")
plot(t, rad2deg(y[:, :5]))
xlabel('Time [s]')
ylabel('Angle [deg]')
legend(["${}$".format(vlatex(c)) for c in coordinates])
```

    Plotting

    <matplotlib.legend.Legend at 0x7f46cc386790>

# Visualization

```python
from pydy.viz.shapes import Cylinder, Sphere
from pydy.viz.scene import Scene
from pydy.viz.visualization_frame import VisualizationFrame

# stack the lengths and use the bodies from kinematics
lengths = [lower_leg_length, upper_leg_length, hip_length, hip_length, hip_length, upper_leg_length, lower_leg_length]
bodies = [lower_leg_left, upper_leg_left, hip, hip, hip, upper_leg_right, lower_leg_right]

viz_frames = []
colors = ['yellow','green','red','red','red','green','blue']

for i, (body, particle, mass_center) in enumerate(zip(bodies, particles, mass_centers)):
#        body_shape = Cylinder(name='cylinder{}'.format(i),
#                              radius=0.05,
#                              length=lengths[i],
#                              color='red')
#
#        viz_frames.append(VisualizationFrame('link_frame{}'.format(i), body,
#                                             body_shape))

    particle_shape = Sphere(name='sphere{}'.format(i),
                            radius=0.06,
                            color=colors[i])

    viz_frames.append(VisualizationFrame('particle_frame{}'.format(i),
                                         body.frame,
                                         particle,
                                         particle_shape))

    mass_center_shape = Sphere(name='sphere{}'.format(i),
                            radius=0.02,
                            color='black')

    viz_frames.append(VisualizationFrame('mass_center_frame{}'.format(i),
                                         body.frame,
                                         mass_center,
                                         mass_center_shape))

target_shape = Sphere(name='sphere{}'.format(i),
                            radius=0.02,
                            color='green')

target_right_leg = Point('target_right_leg')
target_right_leg.set_pos(origin, (x_des[2] * inertial_frame.x)+(x_des[3] * inertial_frame.y))

viz_frames.append(VisualizationFrame('target_frame_right',
                                     inertial_frame,
                                     target_right_leg,
                                     target_shape))

## Now the visualization frames can be passed in to create a scene.
scene = Scene(inertial_frame, origin, *viz_frames)

# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(constants, numerical_constants))
scene.states_symbols = coordinates+speeds
scene.states_trajectories = y

scene.display()
```

    /home/letrend/workspace/roboy-ros-control/python/pydy-resources
    ('Serving HTTP on', '127.0.0.1', 'port', 8001, '...')
    To view visualization, open:

    http://localhost:8001/index.html?load=2017-06-28_02-51-33_scene_desc.json
    Press Ctrl+C to stop server...

# export to c header

```python
t = symbols('t')
a0 = ankle_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k0 = knee_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hl = hip_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hc = hip_center.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hr = hip_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k1 = knee_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
a1 = ankle_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
[(c_name, c_code), (h_name, c_header)] = codegen(
    [("Jacobian",J),
     ("ankle_right_hip_center",F2),
     ("ankle_left", a0),
     ("knee_left", k0),
     ("hip_left", hl),
     ("hip_center", hc),
     ("hip_right", hr),
     ("knee_right", k1),
     ("ankle_right",a1)
    ] ,
    "C", "PaBiRoboy_DanceControl", project='PaBiRoboy_DanceControl',global_vars=(t,lower_leg_length, upper_leg_length, hip_length, theta0,theta1,theta2,theta3),
    header=True, empty=False)
print(c_code)
```

    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /index.html?load=2017-06-28_02-51-33_scene_desc.json HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /css/bootstrap.min.css HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /css/slider.css HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /css/main.css HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /css/codemirror/codemirror.css HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /css/codemirror/blackboard.css HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/jquery/jquery.min.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/jquery/jquery-ui.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/bootstrap/bootstrap.min.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/codemirror/codemirror.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/codemirror/javascript-mode.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/three/three.min.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/three/TrackballControls.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/utils/bootstrap-slider.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/utils/modernizr-2.0.6.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/external/utils/prototype.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/dv.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/scene.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/parser.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/param_editor.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/materials.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:40] "GET /js/dyviz/main.js HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:41] code 404, message File not found
    127.0.0.1 - - [28/Jun/2017 02:51:41] "GET /fonts/glyphicons-halflings-regular.woff2 HTTP/1.1" 404 -
    127.0.0.1 - - [28/Jun/2017 02:51:41] "GET /2017-06-28_02-51-33_scene_desc.json HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:41] "GET /fonts/glyphicons-halflings-regular.woff HTTP/1.1" 200 -
    127.0.0.1 - - [28/Jun/2017 02:51:41] "GET /2017-06-28_02-51-33_simulation_data.json HTTP/1.1" 200 -

    /******************************************************************************
     *                       Code generated with sympy 1.0                        *
     *                                                                            *
     *              See http://www.sympy.org/ for more information.               *
     *                                                                            *
     *               This file is part of 'PaBiRoboy_DanceControl'                *
     ******************************************************************************/
    #include "PaBiRoboy_DanceControl.h"
    #include <math.h>
    void Jacobian(double *out_7040956801295068946) {
       out_7040956801295068946[0] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l2*cos(phi(t) - theta0(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[1] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[2] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t));
       out_7040956801295068946[3] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t));
       out_7040956801295068946[4] = l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l1*cos(phi(t)) - l2*cos(phi(t) - theta0(t)) + l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) + l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[5] = l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l2*sin(phi(t) - theta0(t)) + l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[6] = l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[7] = l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t));
       out_7040956801295068946[8] = l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t));
       out_7040956801295068946[9] = -l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) - l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) + l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[10] = l2*cos(phi(t) - theta0(t)) - 1.0L/2.0L*l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[11] = -1.0L/2.0L*l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[12] = 0;
       out_7040956801295068946[13] = 0;
       out_7040956801295068946[14] = -l1*cos(phi(t)) - l2*cos(phi(t) - theta0(t)) + (1.0L/2.0L)*l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[15] = l2*sin(phi(t) - theta0(t)) - 1.0L/2.0L*l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[16] = -1.0L/2.0L*l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_7040956801295068946[17] = 0;
       out_7040956801295068946[18] = 0;
       out_7040956801295068946[19] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) + (1.0L/2.0L)*l3*cos(-phi(t) + theta0(t) + theta1(t));
    }
    void ankle_right_hip_center(double *out_916151544320618253) {
       out_916151544320618253[0] = -l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) - l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) + l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_916151544320618253[1] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_916151544320618253[2] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) + (1.0L/2.0L)*l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_916151544320618253[3] = l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - 1.0L/2.0L*l3*sin(-phi(t) + theta0(t) + theta1(t));
    }
    void ankle_left(double *out_8826841557315653786) {
       out_8826841557315653786[0] = 0;
       out_8826841557315653786[1] = 0;
       out_8826841557315653786[2] = 0;
    }
    void knee_left(double *out_2247889337447503075) {
       out_2247889337447503075[0] = -l1*sin(phi(t));
       out_2247889337447503075[1] = l1*cos(phi(t));
       out_2247889337447503075[2] = 0;
    }
    void hip_left(double *out_9064110474024648940) {
       out_9064110474024648940[0] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t));
       out_9064110474024648940[1] = l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t));
       out_9064110474024648940[2] = 0;
    }
    void hip_center(double *out_659177576507600807) {
       out_659177576507600807[0] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) + (1.0L/2.0L)*l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_659177576507600807[1] = l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - 1.0L/2.0L*l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_659177576507600807[2] = 0;
    }
    void hip_right(double *out_8420808845996663653) {
       out_8420808845996663653[0] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) + l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_8420808845996663653[1] = l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_8420808845996663653[2] = 0;
    }
    void knee_right(double *out_2877649314090428462) {
       out_2877649314090428462[0] = -l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) - l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) + l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_2877649314090428462[1] = l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_2877649314090428462[2] = 0;
    }
    void ankle_right(double *out_8643290169570541467) {
       out_8643290169570541467[0] = -l1*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) - l1*sin(phi(t)) - l2*sin(phi(t) - theta0(t)) - l2*sin(-phi(t) + theta0(t) + theta1(t) + theta2(t)) + l3*cos(-phi(t) + theta0(t) + theta1(t));
       out_8643290169570541467[1] = -l1*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t) + theta3(t)) + l1*cos(phi(t)) + l2*cos(phi(t) - theta0(t)) - l2*cos(-phi(t) + theta0(t) + theta1(t) + theta2(t)) - l3*sin(-phi(t) + theta0(t) + theta1(t));
       out_8643290169570541467[2] = 0;
    }

```python

```