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