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