def deltaMotorUpdate(self, dm=[]):
self.deltaMotor(dm)
for i in range(25):
self.update()
world.Step(TIME_STEP, vel_iters, pos_iters)
world.ClearForces()
return self.getFinalPos()
def errorMinWorldLoop(self):
global world, TIME_STEP, vel_iters, pos_iters
err = self.update()
normhist, sumerr, niter = [1], 1, 0
while (err > 0.05 and sumerr > 0.01 and niter < 500):
m = self.getJointAngles()
err = self.update()
world.Step(TIME_STEP, vel_iters, pos_iters)
world.ClearForces()
normhist.append(self.dmnorm(m))
if (len(normhist) > 15): normhist.pop(0)
sumerr = sum(normhist)
niter += 1
#if(int(100*(time.time() - start_time)) % 10 == 0):
# print self.which, "err", err, "dmnorm", sum(normhist)
return err