def testRisingEdgeDetectionComplex(self): #sin from 0 to 6.5 pi sinEndsRising = [2.5 * sin(math.pi * float(i) / 4) for i in range(27)] for element in sinEndsRising: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.RISING_EDGE)
def testConvergenceDetection(self): convergent_sequence=[(2.5-float(i)/50)*sin(math.pi*float(i)/10) for i in range(101) ] for element in convergent_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue( self.hpd.hasConverged() )
def testFallingEdgeDetectionComplex(self): #-sin from 0 to 6.5 pi sinEndsFalling=[-2.5*sin(math.pi*float(i)/4) for i in range(27)] for element in sinEndsFalling: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.FALLING_EDGE)
def testLimitCycleDetection(self): lc_sequence=[2.5*sin(math.pi*float(i)/10) for i in range(201)] for element in lc_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue( self.hpd.isLimitCycle() )
def testConvergingDetection(self): converging_sequence=[(11-float(i)/4)*sin(math.pi*float(i)/10) for i in range(41)] for element in converging_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue( self.hpd.isConverging() )
def testInstabilityDetection(self): unstable_sequence=[float(i)/4*sin(math.pi*float(i)/10) for i in range(41)] for element in unstable_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue( self.hpd.isUnstable() )
def testNanGetsSanitized(self): try: global_time.updateDelta(0.1) self.pid.update(float("nan"), 2) self.assertTrue(False) except ValueError as error: self.assertTrue("cannot be NaN" in str(error))
def testMaxMovementRateBounded(self): self.pid.max_movement_rate = 100 try: global_time.updateDelta(0.1) self.pid.update(20, 2) self.assertTrue(False) except ValueError as error: self.assertTrue("unsafe rate" in str(error))
def testInstabilityDetection(self): unstable_sequence = [ float(i) / 4 * sin(math.pi * float(i) / 10) for i in range(41) ] for element in unstable_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue(self.hpd.isUnstable())
def testUpdateITerm(self): self.pid.kp = 0 self.pid.kd = 0 global_time.updateDelta(0.1) small = self.pid.update(.1, 0) global_time.updateDelta(0.1) large = self.pid.update(.1, 0) self.assertTrue(small < large)
def testConvergingDetection(self): converging_sequence = [ (11 - float(i) / 4) * sin(math.pi * float(i) / 10) for i in range(41) ] for element in converging_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue(self.hpd.isConverging())
def testConvergenceDetection(self): convergent_sequence = [ (2.5 - float(i) / 50) * sin(math.pi * float(i) / 10) for i in range(101) ] for element in convergent_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue(self.hpd.hasConverged())
def testUpdateDTerm(self): self.pid.kp = 0 self.pid.ki = 0 global_time.updateDelta(0.1) large = self.pid.update(.1, 0) self.pid.prev_error = .2 global_time.updateDelta(0.1) small = self.pid.update(.1, 0) self.assertTrue(small < large)
def testRisingEdgeHysteresis(self): #tests that a falling edge which does not hit hysteretic #limits is not detected two_cycle_rising=[-2.5*sin(math.pi*float(i)/10 ) for i in range(16)] short_falling_edge=[ (2.5-float(i)/20) for i in range(10)] two_cycle_rising.extend(short_falling_edge) for element in two_cycle_rising: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.RISING_EDGE)
def testRisingEdgeHysteresis(self): #tests that a falling edge which does not hit hysteretic #limits is not detected two_cycle_rising = [ -2.5 * sin(math.pi * float(i) / 10) for i in range(16) ] short_falling_edge = [(2.5 - float(i) / 20) for i in range(10)] two_cycle_rising.extend(short_falling_edge) for element in two_cycle_rising: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.RISING_EDGE)
def testIsDone(self): print "testing isDone" self.path = array([[0, 1, 1, 1], [1, 2, 2, 2], [2, 9, 9, 9], [3, 28, 28, 28]]) self.path = self.path.transpose() self.mock_leg_model.getFootPos().AndReturn([0, 0, 0]) self.mock_leg_model.jointAnglesFromFootPos(ReturnTrue(self.path)) mox.Replay(self.mock_leg_model) ifm = InterpolatedFootMove(self.mock_leg_model, self.path) ifm.update() self.assertFalse(ifm.isDone()) global_time.updateDelta(4) ifm.update() self.assertTrue(ifm.isDone()) mox.Verify(self.mock_leg_model)
def testCubic(self): print "testing cubic interpolation" self.path = array([[0, 1, 1, 1], [1, 2, 2, 2], [2, 9, 9, 9], [3, 28, 28, 28]]) self.path = self.path.transpose() self.mock_leg_model.getFootPos().AndReturn([0, 0, 0]) self.mock_leg_model.jointAnglesFromFootPos( ArraysEqual(array([.0, .0, .0]))) self.mock_leg_model.jointAnglesFromFootPos( ArraysEqual(array([.001, .001, .001]))) self.mock_leg_model.jointAnglesFromFootPos( ArraysEqual(array([.008, .008, .008]))) self.mock_leg_model.jointAnglesFromFootPos( ArraysEqual(array([.027, .027, .027]))) self.mock_leg_model.jointAnglesFromFootPos( ArraysEqual(array([.064, .064, .064]))) mox.Replay(self.mock_leg_model) ifm = InterpolatedFootMove(self.mock_leg_model, self.path) print "TEST 1" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.0, .0, .0]) self.assertFalse(ifm.isDone()) ifm.update() global_time.updateDelta(0.1) print "TEST 2" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.001, .001, .001]) ifm.update() global_time.updateDelta(0.1) print "TEST 3" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.008, .008, .008]) ifm.update() global_time.updateDelta(0.1) print "TEST 4" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.027, .027, .027]) ifm.update() global_time.updateDelta(0.1) print "TEST 5" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.064, .064, .064]) ifm.update() mox.Verify(self.mock_leg_model)
def testCubic(self): print "testing cubic interpolation" self.path = array([[0, 1, 1, 1], [1, 2, 2, 2], [2, 9, 9, 9], [3, 28, 28, 28]]) self.path = self.path.transpose() self.mock_leg_model.getFootPos().AndReturn([0, 0, 0]) self.mock_leg_model.jointAnglesFromFootPos(ArraysEqual(array([.0, .0, .0]))) self.mock_leg_model.jointAnglesFromFootPos(ArraysEqual(array([.001, .001, .001]))) self.mock_leg_model.jointAnglesFromFootPos(ArraysEqual(array([.008, .008, .008]))) self.mock_leg_model.jointAnglesFromFootPos(ArraysEqual(array([.027, .027, .027]))) self.mock_leg_model.jointAnglesFromFootPos(ArraysEqual(array([.064, .064, .064]))) mox.Replay(self.mock_leg_model) ifm = InterpolatedFootMove(self.mock_leg_model, self.path) print "TEST 1" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.0, .0, .0]) self.assertFalse(ifm.isDone()) ifm.update() global_time.updateDelta(0.1) print "TEST 2" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.001, .001, .001]) ifm.update() global_time.updateDelta(0.1) print "TEST 3" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.008, .008, .008]) ifm.update() global_time.updateDelta(0.1) print "TEST 4" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.027, .027, .027]) ifm.update() global_time.updateDelta(0.1) print "TEST 5" #target_foot_pos = array([ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3, ifm.stop_watch.getTime()**3]) print "expected target_foot_pos = ", array([.064, .064, .064]) ifm.update() mox.Verify(self.mock_leg_model)
def _step(self, time, signal): global_time.updateDelta(time) return self.filter.update(signal)
def _test_update(self, signal): """ Increments time by 10 ms and then calculates the response. """ global_time.updateDelta(0.01) return self.update(signal)
def testRisingEdgeDetectionFromInit(self): risingsequence = [float(i) / 10 for i in range(25)] for element in risingsequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.RISING_EDGE)
def testFallingEdgeDetection(self): fallingsequence = [-float(i) / 10 for i in range(25)] for element in fallingsequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.FALLING_EDGE)
def testFallingEdgeDetection(self): fallingsequence=[-float(i)/10 for i in range(25)] for element in fallingsequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.FALLING_EDGE)
def testRisingEdgeDetectionFromInit(self): risingsequence=[float(i)/10 for i in range(25)] for element in risingsequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertEquals(self.hpd.getEdgeType(), self.hpd.RISING_EDGE)
def testLimitCycleDetection(self): lc_sequence = [2.5 * sin(math.pi * float(i) / 10) for i in range(201)] for element in lc_sequence: global_time.updateDelta(0.1) self.hpd.update(element) self.assertTrue(self.hpd.isLimitCycle())
def testUpdateNoop(self): global_time.updateDelta(0.1) self.assertEquals(0, self.pid.update(0, 0))
def testUpdateSmallPositiveError(self): global_time.updateDelta(0.1) self.assertTrue(0 < self.pid.update(.1, 0))
def testUpdateSmallNegativeError(self): global_time.updateDelta(0.1) self.assertTrue(0 > self.pid.update(-.1, 0))