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()
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