from sympy.physics.vector import ReferenceFrame, dot, cross
from sympy import *
import sympy.vector as vector
t, a1, c1, a2, a3, b4, c4, m45, A45,B45, C45, g = sympy.symbols("t a1 c1 a2 a3 b4 c4 m_(45) A_(45) B_(45) C_(45) g")
theta1 = Function("theta1")(t)
theta2 = Function("theta2")(t)
theta3 = Function("theta3")(t)
theta4 = Function("theta4")(t)
N = ReferenceFrame("N_s")
x_s, y_s, z_s = N.x, N.y, N.z
x1, y1, z1 = cos(theta1)*x_s + sin(theta1)*y_s, cos(theta1)*y_s - sin(theta1)*x_s, z_s
x2, z2 = cos(theta2)*x1 - sin(theta2)*z1, cos(theta2)*z1 + sin(theta2)*x1
x3, z3 = cos(theta3)*x2 - sin(theta3)*z2, cos(theta3)*z2 + sin(theta3)*x2
x4, z4 = cos(theta4)*x3 - sin(theta4)*z3, cos(theta4)*z3 + sin(theta4)*x3
Omega_4 = (theta2 + theta3 +theta4)*y1 + theta1*z_s
OO3 = a1*x1 + c1*z1 + a2*x2 + a3*x3
OO4 = OO3 - b4*y1 - c4*z4
O3O4 = OO4 - OO3
VO3 = OO3.diff(t, N)
VO4 = OO4.diff(t, N)
kinetic_angular_momentum_O4 = A45*dot(x4, Omega_4)*x4 + B45*dot(y1, Omega_4)*y1 + C45*dot(z4, Omega_4)*z4 + m45*cross(O3O4, VO4)
dynamic_angular_momentum_scalar_y1 = dot(y1, kinetic_angular_momentum_O4.diff(t, N)) + m45 * dot(y1, cross(VO3, VO4))
g_torque = -m45*g*cross(O3O4, z_s)
Motor_couple = dynamic_angular_momentum_scalar_y1 -  dot(y1, g_torque)
init_printing()
simplify(Motor_couple)