BodyA = Body('BodyA', A, p2, m, I, system) ParticleO = Particle(p2, M, 'ParticleO', system) stretch = q - q0 system.add_spring_force1(k, (stretch) * N.z, wNA) system.addforce(-b * v2, v2) system.addforcegravity(-g * N.y) pos = sympy.cos(system.t * 2 * pi / 2) eq = pos * N.x - p1 eq_d = eq.time_derivative() eq_dd = eq_d.time_derivative() eq_dd_scalar = [] eq_dd_scalar.append(eq_dd.dot(N.x)) system.add_constraint(AccelerationConstraint(eq_dd_scalar)) f, ma = system.getdynamics() func1, lambda1 = system.state_space_post_invert( f, ma, constants=system.constant_values, return_lambda=True) states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, ))
system.addforce(torque * O.z, wOA) system.addforce(-torque * O.z, wOC) # eq = [] eq.append(pBtip - pDtip) eq.append(O.y) eq_d = [item.time_derivative() for item in eq] eq_dd = [item.time_derivative() for item in eq_d] eq_dd_scalar = [] eq_dd_scalar.append(eq_dd[0].dot(N.x)) eq_dd_scalar.append(eq_dd[0].dot(N.y)) eq_dd_scalar.append(eq_dd[1].dot(N.y)) c = AccelerationConstraint(eq_dd_scalar) # c.linearize(0) system.add_constraint(c) # f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, constants=constants, variable_functions={my_signal: ft2}) states = pynamics.integration.integrate(func1, ini, t, rtol=tol, atol=tol) KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE], constant_values=constants) energy.calc(states, t)
ParticleB = Particle(pAB2,mA,'ParticleB',system) system.addforce(-b*vAB,vAB) system.addforce(-b*vAB2,vAB2) system.addforcegravity(-g*N.y) v = pAB2-pNA2 u = (v.dot(v))**.5 eq1 = v.dot(v) - lA**2 eq1_d=system.derivative(eq1) eq1_dd=system.derivative(eq1_d) eq = [eq1_dd] constraint = AccelerationConstraint([eq1_dd]) system.add_constraint(constraint) tol = 1e-4 tinitial = 0 tfinal = 5 tstep = 1/30 t = numpy.r_[tinitial:tfinal:tstep] f,ma = system.getdynamics() func = system.state_space_post_invert(f,ma,constants=system.constant_values) statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables]
pCcm = 0 * N.x IC = Dyadic.build(C, Ixx, Iyy, Izz) w1 = N.get_w_to(C) w2 = wx * C.x + wy * C.y + wz * C.z N.set_w(C, w2) eq0 = w1 - w2 eq0_d = eq0.time_derivative() eq = [] eq.append(eq0_d.dot(B.x)) eq.append(eq0_d.dot(B.y)) eq.append(eq0_d.dot(B.z)) c = AccelerationConstraint(eq) # c.linearize(0) system.add_constraint(c) eq2 = [] eq2.append(eq0.dot(B.x)) eq2.append(eq0.dot(B.y)) eq2.append(eq0.dot(B.z)) k = KinematicConstraint(eq2) variables = [qA_d, qB_d, qC_d] result = k.solve_numeric(variables, [1, 1, 1], system.constant_values) initialvalues.update(result) # for constraint in system.constraints: # constraint.solve()