#system.add_spring_force1(k,(q1)*f1.z,wNA)
points = [particle1.pCM]
points_x = [item.dot(f1.x) for item in points]
points_y = [item.dot(f1.y) for item in points]
points_z = [item.dot(f1.z) for item in points]
output_x = Output(points_x)
output_y = Output(points_y)
output_z = Output(points_z)
f,ma = system.getdynamics()
func = system.state_space_post_invert(f,ma)
t = numpy.r_[0:5:.001]
states=pynamics.integration.integrate_odeint(func,system.get_ini(),t,atol=1e-5,rtol = 1e-5, args=({'constants':system.constant_values},))
x = output_x.calc(states)
y = output_y.calc(states)
z = output_z.calc(states)
KE = system.get_KE()
PE = system.getPEGravity(0*f1.x) - system.getPESprings()
output = Output([KE-PE])
outputs = output.calc(states)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.plot(x,y,z)
plt.axis('equal')
qA, qA_d, qA_dd = Differentiable('qA', ini=[0, 0])
qB, qB_d, qB_dd = Differentiable('qB', ini=[0, 0])
qC, qC_d, qC_dd = Differentiable('qC', ini=[ang_ini * pi / 180, 0])
mC = Constant(.05, 'mC')
g = Constant(9.81, 'g')
I_11 = Constant(6e-3, 'I_11')
rho = Constant(1.292, 'rho')
Sw = Constant(.1, 'Sw')
Se = Constant(.025, 'Se')
l = Constant(.35, 'l')
lw = Constant(-.03, 'lw')
le = Constant(.04, 'le')
qE = Constant(3 * pi / 180, 'qE')
ini = system.get_ini()
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
E = Frame('E')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [1, 0, 0], qA)
B.rotate_fixed_axis_directed(A, [0, 1, 0], qB)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC)
E.rotate_fixed_axis_directed(C, [0, 0, 1], -qE)
pCcm = x * N.x + y * N.y + z * N.z
#pCcm=x*N.x+y*N.y
points = [particle1.pCM]
points_x = [item.dot(f1.x) for item in points]
points_y = [item.dot(f1.y) for item in points]
points_z = [item.dot(f1.z) for item in points]
output_x = Output(points_x)
output_y = Output(points_y)
output_z = Output(points_z)
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma, eq_dd=eq1_dd)
t = numpy.r_[0:5:.001]
states = pynamics.integration.integrate_odeint(func,
system.get_ini(),
t,
atol=1e-5,
rtol=1e-5,
args=({
'constants':
system.constant_values
}, ))
KE = system.get_KE()
PE = system.getPEGravity(0 * f1.x) - system.getPESprings()
output = Output([KE - PE])
x = output_x.calc(states)
y = output_y.calc(states)
