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)