def make_lower_leg(world):
lower_leg_mass = Mass()
lower_leg_mass.setBox(body_density, 30, 100, 5)
lower_leg = Body(world)
lower_leg.setMass(lower_leg_mass)
servo = make_servo(world)
fix(world, lower_leg, servo)
return lower_leg
def __init__(self, world, pos, size, mass):
self.world = world
self.pos = pos
# Create a body inside the world
self.body = Body(world)
M = Mass()
M.setSphere(2500.0, size)
M.mass = mass
self.body.setMass(M)
self.body.setPosition(pos)
# draw body
self.rbody = sphere(pos=pos, radius=size)
def make_body(world):
mass = Mass()
mass.setBox(body_density, 240, 27, 150)
body = Body(world)
body.setMass(mass)
fix(world, body, make_battery(world, (0, 27 / 2, -(150 / 2 - 35 / 2))))
fix(world, body, make_battery(world, (0, 27 / 2, 150 / 2 - 35 / 2)))
fix(world, body, make_servo(world, (240 / 2 - 30, 0, 150 / 2)))
fix(world, body, make_servo(world, (240 / 2 - 30, 0, -150 / 2)))
fix(world, body, make_servo(world, (-(240 / 2 - 30), 0, 150 / 2)))
fix(world, body, make_servo(world, (-(240 / 2 - 30), 0, -150 / 2)))
return body
def __init__(self, gui):
self.gui = gui
self.vDHistory = []
self.bPHistory = []
self.th = TimeHelper()
self.w = World()
self.w.setGravity((0.0, 0.0, 0.0))
self.b = Body(self.w)
self.b.setPosition((0.0, 0.0, 0.0))
self.lt = self.th.getTimestamp(microsec=True)
self.lastVal = None
def __init__(self, world, space, pos, size, mass, color=(0.7, 0.7, 0.7)):
self.world = world
self.pos = pos
# Create a body inside the world
self.body = Body(world)
M = Mass()
M.setSphere(2500, size)
M.mass = mass
self.body.setMass(M)
self.body.setPosition(pos)
if space:
# setect colisions
self.geom = GeomSphere(space, size)
self.geom.setBody(self.body)
# draw body
self.rbody = sphere(pos=pos, radius=size, color=color)
class Sphere(object):
def __init__(self, world, pos, size, mass):
self.world = world
self.pos = pos
# Create a body inside the world
self.body = Body(world)
M = Mass()
M.setSphere(2500.0, size)
M.mass = mass
self.body.setMass(M)
self.body.setPosition(pos)
# draw body
self.rbody = sphere(pos=pos, radius=size)
def getPosition(self):
return self.body.getPosition()
def getLinearVel(self):
return self.body.getLinearVel()
def addForce(self, force):
return self.body.addForce(force)
def static(self):
self.sj = BallJoint(self.world)
self.sj.attach(self.body, environment)
self.sj.setAnchor(self.pos)
def joint(self, body):
self.j = BallJoint(self.world)
self.j.attach(self.body, body.body)
self.j.setAnchor( body.pos )
def draw(self):
self.rbody.pos = self.getPosition()
def make_battery(world, position=(0, 0, 0)):
mass = Mass()
mass.setBox(battery_density, 70, 25, 35)
body = Body(world)
body.setMass(mass)
body.setPosition(position)
return body
def make_servo(world, position=(0, 0, 0)):
mass = Mass()
mass.setBox(servo_density, 40, 20, 35)
body = Body(world)
body.setMass(mass)
body.setPosition(position)
return body
def __init__(self, world, space, pos, size, mass, color=(0.7, 0.7, 0.7)):
self.world = world
self.pos = pos
# Create a body inside the world
self.body = Body(world)
M = Mass()
M.setSphere(2500, size)
M.mass = mass
self.body.setMass(M)
self.body.setPosition(pos)
if space:
# setect colisions
self.geom = GeomSphere(space, size)
self.geom.setBody(self.body)
# draw body
self.rbody = sphere(pos=pos, radius=size, color=color)
def addToODE(self, simulator, x0, y0, z0):
world = simulator.getWorld()
morphology = self.morphology
self.bodies = {}
for part in morphology.getParts():
self.bodies[part] = Body(world)
for part in self.bodies:
self.bodies[part].setMass(morphology.getWeight(part))
#on commence par le bas !
self.bodies['mollet_d'].setPosition(
morphology.getLength('largeur') / 2, 0,
morphology.getLength('mollet') / 2)
self.bodies['mollet_g'].setPosition(
-morphology.getLength('largeur') / 2, 0,
morphology.getLength('mollet') / 2)
self.bodies['cuisse_d'].setPosition(
morphology.getLength('largeur') / 2, 0,
morphology.getLength('mollet') +
morphology.getLength('cuisse') / 2)
self.bodies['cuisse_g'].setPosition(
-morphology.getLength('largeur') / 2, 0,
morphology.getLength('mollet') +
morphology.getLength('cuisse') / 2)
self.bodies['tronc_bassin_bas'].setPosition(
0, 0,
morphology.getLength('mollet') + morphology.getLength('cuisse') +
morphology.getLength('tronc_bassin_bas') / 2)
self.bodies['tronc_bassin_haut'].setPosition(
0, 0,
morphology.getLength('mollet') + morphology.getLength('cuisse') +
morphology.getLength('tronc_bassin_bas') +
morphology.getLength('tronc_bassin_haut') / 2)
class odeRTB:
def __init__(self, gui):
self.gui = gui
self.vDHistory = []
self.bPHistory = []
self.th = TimeHelper()
self.w = World()
self.w.setGravity((0.0, 0.0, 0.0))
self.b = Body(self.w)
self.b.setPosition((0.0, 0.0, 0.0))
self.lt = self.th.getTimestamp(microsec=True)
self.lastVal = None
def plotAnimate(self, i):
xs = []
ys = []
zs = []
for v in self.bPHistory[:-10]:
xs.append(v[0])
ys.append(v[1])
zs.append(v[2])
if len(self.bPHistory) > 500:
self.bPHistory.pop(0)
self.vDHistory.pop(0)
self.ax1.clear()
r = np.array(xs)
s = np.array(ys)
t = np.array(zs)
self.ax1.scatter(r, s, zs, marker='o')
def startPltShow(self, a=''):
self.fig = plt.figure()
self.ax1 = self.fig.add_subplot(111, projection='3d')
ani = animation.FuncAnimation(self.fig, self.plotAnimate, interval=500)
plt.show()
iter = 0
def update(self, fromWho, val):
#print("ode.updateIt",fromWho)
self.iter += 1
if fromWho == 'accel':
if len(self.vDHistory) == 10:
_thread.start_new(self.startPltShow, ())
t = self.th.getTimestamp(microsec=True)
if self.lastVal == None:
self.lastVal = val
else:
vD = []
for k in range(len(val)):
vD.append(self.lastVal[k] - val[k])
self.vDHistory.append(vD)
#print(t-self.lt," mm/s^2:",vD)
#if (self.iter%100)==0:
# self.b.setLinearVel((0.0,0.0,.0))
# print("0.0.0")
bp = self.b.getPosition()
self.bPHistory.append(bp)
#print("b pos: {} {} {}".format(
# round(bp[0], 4),
# round(bp[1], 4),
# round(bp[2], 4)
# ))
avgFrom = 100
if len(self.vDHistory) % avgFrom == 0:
print("recall avg")
vcc = [0.0, 0.0, 0.0]
for ii in range(avgFrom - 1):
vcc[0] += self.vDHistory[ii][0]
vcc[1] += self.vDHistory[ii][1]
vcc[2] += self.vDHistory[ii][2]
vcc[0] /= float(avgFrom - 1.0)
vcc[1] /= float(avgFrom - 1.0)
vcc[2] /= float(avgFrom - 1.0)
self.va = vcc
vc = val
elif len(self.vDHistory) > avgFrom:
vc = [
val[0] - self.va[0], val[1] - self.va[1],
val[2] - self.va[2]
]
#print("va:",self.va)
else:
vc = val
# print("t",(t-self.lt)/1000000.0)
#print("vdh",len(self.vDHistory)," -",vc)
if len(self.vDHistory) > avgFrom + 10:
self.w.setGravity((
vD[0], #vc[0],#vD[0],
vD[1], #vc[1],#vD[1],
vD[2] #vc[2]#vD[2]
))
#self.w.step( (t-self.lt)/1000000.0 )
self.w.step(0.1)
self.lastVal = val
self.lt = t
def make_upper_leg(world):
upper_leg_mass = Mass()
upper_leg_mass.setBox(body_density, 30, 100, 5)
upper_leg = Body(world)
upper_leg.setMass(upper_leg_mass)
return upper_leg
def __init__(self, scene):
self.scene = scene
self.body = Body(scene.world)
self.scene.on_update(self.update)
