from solution.kinetics import *

from sympy import trigsimp
from sympy.physics.mechanics import KanesMethod

from sympy.physics.vector import init_vprinting
init_vprinting()

KanesMethod?

coordinates = [theta1, theta2, theta3]
coordinates

speeds = [omega1, omega2, omega3]
speeds

kinematical_differential_equations

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

loads = [lower_leg_grav_force,
         upper_leg_grav_force,
         torso_grav_force, 
         lower_leg_torque,
         upper_leg_torque,
         torso_torque]
loads

bodies = [lower_leg, upper_leg, torso]
bodies

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

trigsimp(fr + frstar)

mass_matrix = trigsimp(kane.mass_matrix_full)
mass_matrix

forcing_vector = trigsimp(kane.forcing_full)
forcing_vector