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)
