def __init__(self, config_file="body_model.conf", section="BodyModel"): c = ConfigParser() if not path.exists(config_file): print 'Config file %s not found!' % config_file raise IOError c.read(config_file) self.legs = [LegModel() for i in range(NUM_LEGS)] # Leg Offsets self.LEG0_OFFSET_X = c.getfloat(section, "leg0_offset_x") self.LEG0_OFFSET_Y = c.getfloat(section, "leg0_offset_y") self.LEG0_THETA = c.getfloat(section, "leg0_theta") self.LEG1_OFFSET_X = c.getfloat(section, "leg1_offset_x") self.LEG1_OFFSET_Y = c.getfloat(section, "leg1_offset_y") self.CHASSIS_BOTTOM_Z = c.getfloat(section, "chassis_bottom_z")
def setUp(self): installArrayTypeEqualityFunction(self) self.leg = LegModel() self.leg_state = [array([0.0, 0.0, 0.0]), 0.0]
class LegModelTestCase(unittest.TestCase): def setUp(self): installArrayTypeEqualityFunction(self) self.leg = LegModel() self.leg_state = [array([0.0, 0.0, 0.0]), 0.0] def tearDown(self): pass def testLinkLengthsAreSane(self): l = self.leg self.assertGreater(l.YAW_LEN, 0.0) self.assertGreater(l.THIGH_LEN, 0.0) self.assertGreater(l.CALF_LEN, 0.0) # Thigh should be 5ish times as long as the yaw stub self.assertLess(abs(l.THIGH_LEN / l.YAW_LEN - 5.0), 2.0) # Thigh should be pretty much the same length as the calf self.assertLess(abs(l.THIGH_LEN / l.CALF_LEN - 1.0), 0.3) def testMeasurementOutOfSoftRangeError(self): try: self.leg.setSensorReadings(0, -5, 0, 9e9) # should error even though 0 is in range because -5 is not self.assertTrue(False) except ValueError as error: self.assertTrue("Measured angle out of soft range!" in str(error)) try: self.leg.setSensorReadings(0, 5, 0, 9e9) # should error even though 0 is in range because 5 is not self.assertTrue(False) except ValueError as error: self.assertTrue("Measured angle out of soft range!" in str(error)) # The good case where the measurement is not out of range is covered by other tests def testFootPosFromLegStateZeroAnglesIsSumOfLengthsInX(self): l = self.leg self.assertEqual( array([l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN, 0.0, 0.0]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStatePosRightAngles(self): l = self.leg self.leg_state[0][YAW] = pi_2 self.assertEqual( array([0.0, (l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN), 0.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][HP] = pi_2 self.assertEqual( array([0.0, l.YAW_LEN, -(l.THIGH_LEN + l.CALF_LEN)]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][KP] = pi_2 self.assertEqual( array([0.0, l.YAW_LEN - l.CALF_LEN, -l.THIGH_LEN]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStateShockDepthModifiesCalfLen(self): l = self.leg self.leg_state[0][KP] = pi_2 self.leg_state[1] = 0.1234 self.assertEqual( array([l.YAW_LEN + l.THIGH_LEN, 0.0, -(l.CALF_LEN - 0.1234)]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStateComplex(self): l = self.leg self.leg_state[0][YAW] = -pi_4 self.assertEqual( array([(l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN) / 2**0.5, -(l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN) / 2**0.5, 0.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][HP] = -pi / 6.0 self.assertEqual( array([(l.YAW_LEN + (l.THIGH_LEN + l.CALF_LEN) * 3.0**0.5/2.0) / 2**0.5, -(l.YAW_LEN + (l.THIGH_LEN + l.CALF_LEN) * 3.0**0.5/2.0) / 2**0.5, (l.THIGH_LEN + l.CALF_LEN) / 2.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][KP] = pi / 6.0 self.assertEqual( array([(l.YAW_LEN + l.THIGH_LEN * 3.0**0.5/2.0 + l.CALF_LEN) / 2**0.5, -(l.YAW_LEN + l.THIGH_LEN * 3.0**0.5/2.0 + l.CALF_LEN) / 2**0.5, l.THIGH_LEN / 2.0]), l.footPosFromLegState(self.leg_state)) def testJointAnglesFromFootPosInvertsFootPosFromLegState(self): l = self.leg ls = self.leg_state NUM_SAMPLES = 8 TOL = 1e-4 # Stay away from numerical boundary conditions that are out # of out of our joint range anyway... for yaw in linspace(-pi_2 + TOL, pi_2 - TOL, NUM_SAMPLES): ls[0][YAW] = yaw for hip_pitch in linspace(-pi_2 + TOL, pi_2 - TOL, NUM_SAMPLES): ls[0][HP] = hip_pitch for knee_pitch in linspace(TOL, pi - TOL, NUM_SAMPLES): ls[0][KP] = knee_pitch # Only test a few shock depths for shock_depth in linspace(0.0, 0.02, 2): ls[1] = shock_depth self.assertEqual( ls[0], l.jointAnglesFromFootPos( l.footPosFromLegState(ls), ls[1])) def testShockDepthThredholdsAreSane(self): l = self.leg self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_HIGH, 0.0) self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_LOW, 0.0) self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_HIGH, l.SHOCK_DEPTH_THRESHOLD_LOW) def testFootOnGroundDefaultsToFalse(self): self.assertFalse(self.leg.isFootOnGround()) def testFootOnGroundTurnsOn(self): l = self.leg for sd in arange(0.0, 0.1, 0.001): l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertEqual(sd > l.SHOCK_DEPTH_THRESHOLD_HIGH, l.isFootOnGround()) def testFootOnGroundTurnsOff(self): l = self.leg for sd in arange(0.1, 0.0, -0.001): l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertEqual(sd > l.SHOCK_DEPTH_THRESHOLD_LOW, l.isFootOnGround()) def testFootOnGroundNoiseImmunity(self): l = self.leg time = arange(0.0, 1.0, 0.001) amp = (l.SHOCK_DEPTH_THRESHOLD_HIGH - l.SHOCK_DEPTH_THRESHOLD_LOW - 0.001) / 2.0 offset = (l.SHOCK_DEPTH_THRESHOLD_HIGH + l.SHOCK_DEPTH_THRESHOLD_LOW) / 2.0 shock_depth = amp * sin(2*pi*time * 10.0) + offset l.setSensorReadings(0.0, 0.0, 0.0, 0.0) l.updateFootOnGround() for sd in shock_depth: l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertFalse(l.isFootOnGround()) l.setSensorReadings(0.0, 0.0, 0.0, l.SHOCK_DEPTH_THRESHOLD_HIGH * 2.0) l.updateFootOnGround() for sd in shock_depth: l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertTrue(l.isFootOnGround())
class LegModelTestCase(unittest.TestCase): def setUp(self): installArrayTypeEqualityFunction(self) self.leg = LegModel() self.leg_state = [array([0.0, 0.0, 0.0]), 0.0] def tearDown(self): pass def testLinkLengthsAreSane(self): l = self.leg self.assertGreater(l.YAW_LEN, 0.0) self.assertGreater(l.THIGH_LEN, 0.0) self.assertGreater(l.CALF_LEN, 0.0) # Thigh should be 5ish times as long as the yaw stub self.assertLess(abs(l.THIGH_LEN / l.YAW_LEN - 5.0), 2.0) # Thigh should be pretty much the same length as the calf self.assertLess(abs(l.THIGH_LEN / l.CALF_LEN - 1.0), 0.3) def testMeasurementOutOfSoftRangeError(self): try: self.leg.setSensorReadings( 0, -5, 0, 9e9 ) # should error even though 0 is in range because -5 is not self.assertTrue(False) except ValueError as error: self.assertTrue("Measured angle out of soft range!" in str(error)) try: self.leg.setSensorReadings( 0, 5, 0, 9e9) # should error even though 0 is in range because 5 is not self.assertTrue(False) except ValueError as error: self.assertTrue("Measured angle out of soft range!" in str(error)) # The good case where the measurement is not out of range is covered by other tests def testFootPosFromLegStateZeroAnglesIsSumOfLengthsInX(self): l = self.leg self.assertEqual( array([l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN, 0.0, 0.0]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStatePosRightAngles(self): l = self.leg self.leg_state[0][YAW] = pi_2 self.assertEqual( array([0.0, (l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN), 0.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][HP] = pi_2 self.assertEqual(array([0.0, l.YAW_LEN, -(l.THIGH_LEN + l.CALF_LEN)]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][KP] = pi_2 self.assertEqual(array([0.0, l.YAW_LEN - l.CALF_LEN, -l.THIGH_LEN]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStateShockDepthModifiesCalfLen(self): l = self.leg self.leg_state[0][KP] = pi_2 self.leg_state[1] = 0.1234 self.assertEqual( array([l.YAW_LEN + l.THIGH_LEN, 0.0, -(l.CALF_LEN - 0.1234)]), l.footPosFromLegState(self.leg_state)) def testFootPosFromLegStateComplex(self): l = self.leg self.leg_state[0][YAW] = -pi_4 self.assertEqual( array([(l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN) / 2**0.5, -(l.YAW_LEN + l.THIGH_LEN + l.CALF_LEN) / 2**0.5, 0.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][HP] = -pi / 6.0 self.assertEqual( array([(l.YAW_LEN + (l.THIGH_LEN + l.CALF_LEN) * 3.0**0.5 / 2.0) / 2**0.5, -(l.YAW_LEN + (l.THIGH_LEN + l.CALF_LEN) * 3.0**0.5 / 2.0) / 2**0.5, (l.THIGH_LEN + l.CALF_LEN) / 2.0]), l.footPosFromLegState(self.leg_state)) self.leg_state[0][KP] = pi / 6.0 self.assertEqual( array([(l.YAW_LEN + l.THIGH_LEN * 3.0**0.5 / 2.0 + l.CALF_LEN) / 2**0.5, -(l.YAW_LEN + l.THIGH_LEN * 3.0**0.5 / 2.0 + l.CALF_LEN) / 2**0.5, l.THIGH_LEN / 2.0]), l.footPosFromLegState(self.leg_state)) def testJointAnglesFromFootPosInvertsFootPosFromLegState(self): l = self.leg ls = self.leg_state NUM_SAMPLES = 8 TOL = 1e-4 # Stay away from numerical boundary conditions that are out # of out of our joint range anyway... for yaw in linspace(-pi_2 + TOL, pi_2 - TOL, NUM_SAMPLES): ls[0][YAW] = yaw for hip_pitch in linspace(-pi_2 + TOL, pi_2 - TOL, NUM_SAMPLES): ls[0][HP] = hip_pitch for knee_pitch in linspace(TOL, pi - TOL, NUM_SAMPLES): ls[0][KP] = knee_pitch # Only test a few shock depths for shock_depth in linspace(0.0, 0.02, 2): ls[1] = shock_depth self.assertEqual( ls[0], l.jointAnglesFromFootPos(l.footPosFromLegState(ls), ls[1])) def testShockDepthThredholdsAreSane(self): l = self.leg self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_HIGH, 0.0) self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_LOW, 0.0) self.assertGreater(l.SHOCK_DEPTH_THRESHOLD_HIGH, l.SHOCK_DEPTH_THRESHOLD_LOW) def testFootOnGroundDefaultsToFalse(self): self.assertFalse(self.leg.isFootOnGround()) def testFootOnGroundTurnsOn(self): l = self.leg for sd in arange(0.0, 0.1, 0.001): l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertEqual(sd > l.SHOCK_DEPTH_THRESHOLD_HIGH, l.isFootOnGround()) def testFootOnGroundTurnsOff(self): l = self.leg for sd in arange(0.1, 0.0, -0.001): l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertEqual(sd > l.SHOCK_DEPTH_THRESHOLD_LOW, l.isFootOnGround()) def testFootOnGroundNoiseImmunity(self): l = self.leg time = arange(0.0, 1.0, 0.001) amp = (l.SHOCK_DEPTH_THRESHOLD_HIGH - l.SHOCK_DEPTH_THRESHOLD_LOW - 0.001) / 2.0 offset = (l.SHOCK_DEPTH_THRESHOLD_HIGH + l.SHOCK_DEPTH_THRESHOLD_LOW) / 2.0 shock_depth = amp * sin(2 * pi * time * 10.0) + offset l.setSensorReadings(0.0, 0.0, 0.0, 0.0) l.updateFootOnGround() for sd in shock_depth: l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertFalse(l.isFootOnGround()) l.setSensorReadings(0.0, 0.0, 0.0, l.SHOCK_DEPTH_THRESHOLD_HIGH * 2.0) l.updateFootOnGround() for sd in shock_depth: l.setSensorReadings(0.0, 0.0, 0.0, sd) l.updateFootOnGround() self.assertTrue(l.isFootOnGround())