Section2 Single Leg Model
import sympy
from sympy import *
sympy.init_printing()
import math
import numpy as np
x, z, theta = symbols('x z theta')
a, k, h = symbols('a k h')
tauK, tauH = symbols('tauK tauH')
L1, L2 = symbols('L1 L2')
fx, fz, ftheta = symbols('fx, fz, ftheta')
# XAB Vector, Equation 1
eq1_1 = Eq(x , -L1*sin(a)-L2*sin(a+k))
eq1_2 = Eq(z , L1*cos(a) + L2*cos(a+k))
eq1_3 = Eq(theta , -h -k -a)
#JAB, Equation 2,3,4
JAB = Matrix([eq1_1.rhs,eq1_2.rhs,eq1_3.rhs]).jacobian(Matrix([a,k,h]))
JAB = transpose(JAB)
# print(JAB)
#Equation 5
Vec_tau = Matrix([0,tauK,tauH])
eq5_1 = JAB * Matrix([fx, fz, ftheta])
equations_5 = [Eq(eq5_1[i], Vec_tau[i]) for i in range(eq5_1.rows)]
# print(equations_5)
# Eq6
eq6_1 = -1/(L1*cos(a) + L2*cos(a+k))*Matrix([[L1*sin(a)+L2*sin(a+k),1]])*Matrix([fz,ftheta])
eq6_1 = -L2*(-ftheta/(L1*cos(a) + L2*cos(a + k)) - fz*(L1*sin(a) + L2*sin(a + k))/(L1*cos(a) + L2*cos(a + k)))*cos(a + k)
equations_5[1]=equations_5[1].subs(fx,eq6_1)
# print(equations_5[1])
# Eq7
eq7_1 = equations_5[1].lhs
eq7_2 = equations_5[2].lhs
print(eq7_1,eq7_2)
