def setupObjects():
global arb, gravity
x = RigidBody(mass1, initialPosition1, shape=size1)
x.grounded = True
y = RigidBody(mass2, initialPosition2, theta=initAngle, shape=size2)
y.grounded = True
arb = ArticulatedBody()
arb.addBody(x)
arb.addBody(y)
arb.add2BodyConstraint((0, r2, 1, r3))
arb.add1BodyConstraint((0, r1))
g1 = tuple(x.mass*comp for comp in gravity)
g2 = tuple(y.mass*comp for comp in gravity)
arb.addExternalForce(0, (g1, (0,0)))
arb.addExternalForce(1, (g2, (0,0)))
arb.finalize()
print arb.matrix
print arb.externalForces
def setupObjects():
global arb, gravity
arb = ArticulatedBody()
ll1 = RigidBody(mass = llmass, pos = ll1pos, shape=llsize)
ul1 = RigidBody(mass = ulmass, pos = ul1pos, shape=ulsize, theta=initUlAngle)
torso = RigidBody(mass = torsomass, pos = torsopos, shape = torsosize)
ul2 = RigidBody(mass = ulmass, pos = ul2pos, shape=ulsize, theta=-initUlAngle)
ll2 = RigidBody(mass = llmass, pos = ll2pos, shape=llsize)
ua1 = RigidBody(mass = uamass, pos = ua1pos, shape=uasize, theta=initUaAngle)
la1 = RigidBody(mass = lamass, pos = la1pos, shape=lasize, theta=initLaAngle)
ua2 = RigidBody(mass = uamass, pos = ua2pos, shape=uasize, theta=-initUaAngle)
la2 = RigidBody(mass = lamass, pos = la2pos, shape=lasize, theta=-initLaAngle)
head = RigidBody(mass = headmass, pos = headpos, shape=headsize)
arb.addBody(ll1)
arb.addBody(ul1)
arb.addBody(torso)
arb.addBody(ul2)
arb.addBody(ll2)
arb.addBody(ua1)
arb.addBody(la1)
arb.addBody(ua2)
arb.addBody(la2)
arb.addBody(head)
gll = tuple(llmass*comp for comp in gravity)
gul = tuple(ulmass*comp for comp in gravity)
gtorso = tuple(torsomass*comp for comp in gravity)
gua = tuple(uamass*comp for comp in gravity)
gla = tuple(lamass*comp for comp in gravity)
ghead = tuple(headmass*comp for comp in gravity)
arb.addExternalForce(0, (gll, (0.0,0.0)))
arb.addExternalForce(4, (gll, (0.0,0.0)))
arb.addExternalForce(1, (gul, (0.0,0.0)))
arb.addExternalForce(3, (gul, (0.0,0.0)))
arb.addExternalForce(2, (gtorso, (0.0,0.0)))
arb.addExternalForce(5, (gua, (0.0,0.0)))
arb.addExternalForce(7, (gua, (0.0,0.0)))
arb.addExternalForce(6, (gla, (0.0,0.0)))
arb.addExternalForce(8, (gla, (0.0,0.0)))
arb.addExternalForce(9, (ghead, (0.0,0.0)))
arb.add1BodyConstraint((0, llr1))
arb.add1BodyConstraint((4, llr1))
arb.add2BodyConstraint((0, llr2, 1, ulr1))
arb.add2BodyConstraint((4, llr2, 3, ulr1))
arb.add2BodyConstraint((1, ulr2, 2, torsor1))
arb.add2BodyConstraint((3, ulr2, 2, torsor2))
arb.add2BodyConstraint((5, uar1, 2, torsor3))
arb.add2BodyConstraint((5, uar2, 6, lar1))
arb.add2BodyConstraint((7, (-uar1[0], uar1[1]), 2, torsor4))
arb.add2BodyConstraint((7, (-uar2[0], uar2[1]), 8, (-lar1[0], lar1[1])))
arb.add2BodyConstraint((2, torsor5, 9, headr))
arb.finalize()
print arb.matrix
print arb.externalForces
