def setupObjects():
global bodies, stencilMat
x = RigidBody(3.0, (3.0,size1[1]/2.0), shape=size1)
x.grounded = True
ypos = [ x.pos[0] + x.shape[0]/2.0 + size2[0]*math.cos(radians(initAngle))/2.0,
x.pos[1] + x.shape[1]/2.0 + size2[0]*math.sin(radians(initAngle))/2.0]
y = RigidBody(1.0, ypos, theta=initAngle, shape=size2)
bodies = [x, y]
stencilMat[7, 2] = -y.I
print stencilMat
def computeForces():
global bodies, r, gravity, stencilMat
bodies[0].clearForces()
#rotate r4 to global frame:
rg = [r[0]*math.cos(radians(bodies[0].theta)) -
r[1]*math.sin(radians(bodies[0].theta)),
r[0]*math.sin(radians(bodies[0].theta)) +
r[1]*math.cos(radians(bodies[0].theta))]
#print rg
Amat = np.copy(stencilMat)
Amat[2,3] = rg[1]
Amat[2,4] = -rg[0]
Amat[3,2] = rg[1]
Amat[4,2] = -rg[0]
#print Amat
b = np.zeros((5,1))
omega = bodies[0].L/bodies[0].I
b[1,0] = mass*gravity[1]
b[3,0] = -(omega**2 * rg[0])
b[4,0] = -(omega**2 * rg[1])
#print b
soln = np.linalg.solve(Amat, b)
#print soln
#print np.dot(Amat,soln)
gforce = [comp*bodies[0].mass for comp in gravity]
bodies[0].addForce(gforce, (0.0,0.0))
bodies[0].addForce(tuple(soln[3:]), r)
def computeForces():
global bodies, r1, r2, r3, r4, gravity, stencilMat
bodies[0].clearForces()
bodies[1].clearForces()
#rotate r4 to global frame:
r4g = [r4[0]*math.cos(radians(bodies[1].theta)) -
r4[1]*math.sin(radians(bodies[1].theta)),
r4[0]*math.sin(radians(bodies[1].theta)) +
r4[1]*math.cos(radians(bodies[1].theta))]
Amat = np.copy(stencilMat)
Amat[2, 3:] = np.array([[-r1[1], r1[0], -r2[1], r2[0], -r3[1], r3[0]]])
Amat[3, 2] = -r4g[1]
Amat[4,2] = r4g[0]
Amat[7, 7:] = np.array([[r4g[1], -r4g[0]]])
print Amat
b = np.zeros((8,1))
b[1,0] = -mass1*gravity[1]
b[6,0] = mass2*gravity[1]
print b
soln, res, rank, sv = np.linalg.lstsq(Amat, b)
bodies[0].addForce(gravity, (0.0,0.0))
bodies[1].addForce(gravity, (0.0,0.0))
print soln, rank
#print soln[3:5]
print Amat*np.matrix(soln)
bodies[0].addForce(tuple(soln[3:5,0]), r1)
bodies[0].addForce(tuple(soln[5:7,0]), r2)
bodies[0].addForce(tuple(soln[7:]), r3)
bodies[1].addForce((-soln[7,0], -soln[8,0]), r4)
def rotate(vec, theta):
return [vec[0]*math.cos(radians(theta)) -
vec[1]*math.sin(radians(theta)),
vec[0]*math.sin(radians(theta)) +
vec[1]*math.cos(radians(theta))]
size1 = (1.0, 5.0)
mass1 = 3.0
size2 = (2.0, 3.0)
mass2 = 1.0
r1 = [0, -size1[1]/2.0]
r2 = [0, size1[1]/2.0]
r3 = [0, -size2[1]/2.0]
initialPosition1 = ((worldSize[0] + size1[0])/2.0, size1[1]/2.0 + worldSize[1]/2.0)
initAngle = 10.0
initialPosition2 = (initialPosition1[0] -
.5*size2[1]*math.sin(radians(initAngle)),
initialPosition1[1] + size1[1]/2.0 +
.5*size2[1]*math.cos(radians(initAngle))
)
actuationTorque = 0.0
#r = (-size[0]/2.0, -size[1]/2.0)#local vector, global computed each step
gravity = (0, -9.81)
stencilMat = np.matrix(np.zeros((10,10), dtype="float"))
stencilMat[0,6] = -1.0
stencilMat[0,8] = -1.0
stencilMat[1,7] = -1.0
torsor2 = (torsosize[0]/2.0, -torsosize[1]/2.0)
torsor3 = (-torsosize[0]/2.0, torsosize[1]/2.0)
torsor4 = (torsosize[0]/2.0, torsosize[1]/2.0)
torsor5 = (0.0, torsosize[1]/2.0)
headmass = 8.0
headsize = (.15, .25)
headr = (0.0, -headsize[1]/2.0)
initUlAngle = -10.0
initUaAngle = 15.0
initLaAngle = 60.0
ll1pos = (1.5, llsize[1]/2.0)
ul1pos = (ll1pos[0] - ulsize[1]*0.5*math.sin(radians(initUlAngle)),
ll1pos[1] + llsize[1]/2.0 + ulsize[1]*0.5*math.cos(radians(initUlAngle)))
torsopos = (ul1pos[0] - .5*math.sin(radians(initUlAngle))*ulsize[1] +
.5*torsosize[0],
ul1pos[1] + .5*math.cos(radians(initUlAngle))*ulsize[1] +
.5*torsosize[1])
ul2pos = (torsopos[0] + torsosize[0]*.5 - ulsize[1]*.5*
math.sin(radians(initUlAngle)),
torsopos[1] - torsosize[1]*.5 - ulsize[1]*.5*
math.cos(radians(initUlAngle)))
ll2pos = (ul2pos[0] - ulsize[1]*.5*math.sin(radians(initUlAngle)),
ul2pos[1] - ulsize[1]*.5*math.cos(radians(initUlAngle)) - llsize[1]/2.0)
