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