def testRadianDegree(self):
self.assertEquals(20, math.Degree(20).valueDegrees())
rad = math.Radian(math.Degree(20))
self.assertEquals(20, rad.valueDegrees())
deg = math.Degree(math.Radian(pmath.pi))
self.assertAlmostEqual(180, deg.valueDegrees(), 3)
# Ensure no automatic double to Radian/Degree conversions occur
try:
deg - 1
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
deg - 2.0
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
rad - 3
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
rad - 4.0
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
def testRight(self):
m = motion.basic.ChangeHeading(-30, 5)
self.qeventHub.subscribeToType(motion.basic.Motion.FINISHED,
self.handleFinished)
# First step
self.motionManager.setMotion(m)
for i in xrange(6, 36, 6):
# Make sure we didn't finish early
self.assert_(not self.motionFinished)
# Make sure the proper depth was commanded
self.assertAlmostEqual(-6, self.controller.yawChange, 2)
# Say we have reached the depth to keep going
self.controller.publishAtOrientation(
math.Quaternion(math.Radian(math.Degree(-i)),
math.Vector3.UNIT_Z))
self.qeventHub.publishEvents()
self.assert_(self.motionFinished)
def enter(self):
self._angleThreshold = self._config.get('threshold', 25)
# Find the angle
vehiclePos = self.vehicle.getPosition()
buoyPos = self.vehicle.getPosition('buoy')
posVect = buoyPos - vehiclePos
posVect.normalise()
# Unit vector is based on vehicle's current orientation
current = self.vehicle.getOrientation().getYaw().valueRadians() + \
(pmath.pi / 2)
unit = math.Vector2(pmath.cos(current), pmath.sin(current))
unit.normalise()
angle = math.Radian(pmath.acos(posVect.dotProduct(unit)))
if abs(angle.valueDegrees()) < self._angleThreshold:
self.publish(DeterminePath.FORWARD, core.Event())
else:
self.publish(DeterminePath.ZIGZAG, core.Event())
def __init__(self, initialValue, finalValue, timePeriod, initialTime=0):
assert isInstance(initialValue, math.Quaternion), \
'SlerpTrajectory: initialValue must be a Quaternion'
assert isInstance(finalValue, math.Quaternion), \
'SlerpTrajectory: finalValue must be a Quaternion'
self._initialValue = initialValue
self._finalValue = finalValue
self._initialTime = initialTime
self._timePeriod = timePeriod
self._finalTime = initialTime + timePeriod
if initialTime == 0:
self._relative = True
# slerp is constant angular velocity so lets
# calculate it once here
rotQuat = initialValue.errorQuaternion(finalValue)
angle = math.Radian(0)
axis = math.Vector3.ZERO
rotQuat.ToAxisAngle(angle, axis)
self._angularVelocity = angle / self._timePeriod