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