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 }, ))
w1 = N.getw_(C) w2 = wx * C.x + wy * C.y + wz * C.z N.set_w(C, w2) from pynamics.constraint import Constraint eq0 = w1 - w2 eq = [] eq.append(eq0.dot(B.x)) eq.append(eq0.dot(B.y)) eq.append(eq0.dot(B.z)) c = Constraint(eq) c.linearize(0) system.add_constraint(c) for constraint in system.constraints: constraint.solve([wx, wy, wz], [qA_d, qB_d, qC_d]) BodyC = Body('BodyC', C, pCcm, mC, IC) system.addforcegravity(-g * N.y) points = [1 * C.x, 0 * C.x, 1 * C.y, 0 * C.y, 1 * C.z] f, ma = system.getdynamics() func1 = system.state_space_pre_invert(f, ma) # func1 = system.state_space_post_invert(f,ma)