def changedLane(self):
if not self._changeMotion(
self._currentDesiredPos,
math.Vector2(self._lane.getX(), self._lane.getY())):
return
self._currentDesiredPos = math.Vector2(self._lane.getX(),
self._lane.getY())
self.motionManager.stopCurrentMotion()
depth = self.stateEstimator.getEstimatedDepth()
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue=depth,
finalValue=depth - self._lane.getY(),
initialRate=self.stateEstimator.getEstimatedDepthRate(),
avgRate=self._diveRate)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(0, self._lane.getX()),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
diveMotion = motion.basic.ChangeDepth(trajectory=diveTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame=Frame.LOCAL)
self.motionManager.setMotion(diveMotion, translateMotion)
def testFinish(self):
""" Make sure that reaching the destination finishes the motion """
self._finished = False
# Create a function to be called to ensure the motion was finished
def _handler(event):
self._finished = True
# Subscribe to the Motion.FINISHED event
self.eventHub.subscribeToType(motion.basic.Motion.FINISHED, _handler)
self.vehicle.position = math.Vector2(0, 0)
self.vehicle.orientation = math.Quaternion.IDENTITY
m = self.makeClass(desiredHeading=0, speed=6, distance=5)
self.motionManager.setMotion(m)
# Make sure the motion hasn't already finished
self.assert_(not self._finished)
self.assertAlmostEqual(6, self.controller.speed, 5)
self.assertAlmostEqual(0, self.controller.sidewaysSpeed, 5)
# Change the position and publish an update
position = math.Vector2(0, 5)
self.vehicle.publishPositionUpdate(position)
self.qeventHub.publishEvents()
# Check that the vehicle is not moving and the motion is finished
self.assertAlmostEqual(0, self.controller.speed, 5)
self.assertAlmostEqual(0, self.controller.sidewaysSpeed, 5)
self.assert_(self._finished)
def testVector2CubicTrajectory(self):
# first make sure that the trajectory boundary conditions are right
iVal = math.Vector2(1, 2)
fVal = math.Vector2(3, 4)
iRate = math.Vector2(0, 0)
fRate = math.Vector2(0, 0)
t = trajectories.Vector2CubicTrajectory(initialValue = iVal,
finalValue = fVal,
initialRate = iRate,
finalRate = fRate)
tTimeI = t.getInitialTime()
tTimeF = t.getFinalTime()
self.assertAlmostEqual(tTimeI, 0.0, 4)
self.assertAlmostEqual(t.computeValue(tTimeI)[0], iVal[0], 4)
self.assertAlmostEqual(t.computeValue(tTimeI)[1], iVal[1], 4)
self.assertAlmostEqual(t.computeValue(tTimeF)[0], fVal[0], 4)
self.assertAlmostEqual(t.computeValue(tTimeF)[1], fVal[1], 4)
self.assertAlmostEqual(t.computeDerivative(tTimeI, 1)[0], iRate[0], 4)
self.assertAlmostEqual(t.computeDerivative(tTimeI, 1)[1], iRate[1], 4)
self.assertAlmostEqual(t.computeDerivative(tTimeF, 1)[0], fRate[0], 4)
self.assertAlmostEqual(t.computeDerivative(tTimeF, 1)[1], fRate[1], 4)
def __init__(self, config, eventHub, vehicle):
self._name = config['name']
SimDevice.__init__(self)
device.IVelocitySensor.__init__(self, eventHub, self._name)
simDevice = vehicle.getDevice('SimulationDevice')
self.robot = simDevice.robot
# This will keep track of the current velocity
self.oldPos = math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
self.velocity = math.Vector2(0, 0)
def __init__(self, eventHub=core.EventHub(), cfg=None):
estimation.IStateEstimator.__init__(self, "StateEstimator", eventHub)
self._position = math.Vector2(0, 0)
self._velocity = math.Vector2(0, 0)
self._orientation = math.Quaternion.IDENTITY
self._depth = 0
self.linAccel = math.Vector3.ZERO
self.angRate = math.Vector3.ZERO
self.depthRate = 0
self._obstacles = {}
def update(self, time):
"""
Sends out events using the current estimated values
"""
self.depthFilter.addValue(
-3.281 * self.robot._main_part._node.position.z)
self.posXFilter.addValue(
self.robot._main_part._node.position.x)
self.posYFilter.addValue(
-self.robot._main_part._node.position.y)
self.position = math.Vector2(
self.posXFilter.getValue(),
self.posYFilter.getValue())
self.velocity = math.Vector2(
self.posXFilter.getValue(1, time),
self.posYFilter.getValue(1, time))
self.depth = self.depthFilter.getValue()
self.depthRate = self.depthFilter.getValue(1, time)
# Package all events
devent = math.NumericEvent()
devent.number = self.getEstimatedDepth()
drevent = math.NumericEvent()
drevent.number = self.getEstimatedDepthRate()
pevent = math.Vector2Event()
pevent.vector2 = self.getEstimatedPosition()
vevent = math.Vector2Event()
vevent.vector2 = self.getEstimatedVelocity()
oevent = math.OrientationEvent()
oevent.orientation = self.getEstimatedOrientation()
# Send all events at the same time
self.publish(estimation.IStateEstimator.ESTIMATED_DEPTH_UPDATE, devent)
self.publish(estimation.IStateEstimator.ESTIMATED_DEPTHRATE_UPDATE,
drevent)
self.publish(estimation.IStateEstimator.ESTIMATED_POSITION_UPDATE,
pevent)
self.publish(estimation.IStateEstimator.ESTIMATED_VELOCITY_UPDATE,
vevent)
self.publish(estimation.IStateEstimator.ESTIMATED_ORIENTATION_UPDATE,
oevent)
def enter(self):
self._orientation = self.ai.data['buoyOrientation']
buoy = self.ai.data['buoyList'].pop(0)
buoyObstacle = getObstacleType(buoy)
buoyLocation = self.stateEstimator.getObstacleLocation(buoyObstacle)
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue = self.stateEstimator.getEstimatedDepth(),
finalValue = buoyLocation.z,
initialRate = self.stateEstimator.getEstimatedDepthRate(),
avgRate = self._diveRate)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = self.stateEstimator.getEstimatedPosition(),
finalValue = math.Vector2(buoyLocation.y, buoyLocation.x),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._speed)
# Dive and translate
diveMotion = motion.basic.ChangeDepth(
trajectory = diveTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.GLOBAL)
self.motionManager.setMotion(diveMotion, translateMotion)
def getPosition(self):
# Gets the exact position relative to the initial position
#currentPos = math.Vector2(-self.robot._main_part._node.position.y,
# self.robot._main_part._node.position.x)
#return currentPos - self.initialPos
return math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
def enter(self):
# Make sure the red light detector is on
self.visionSystem.redLightDetectorOn()
# Get the estimated distance to the object
vehiclePos = self.vehicle.getPosition()
lightPos = self.vehicle.getPosition('buoy')
length = (lightPos - vehiclePos).length()
turnSpeed = self._config.get('turnSpeed', 10)
speed = self._config.get('speed', 3)
offset = self._config.get('offset', 1)
length -= offset
# Turn toward the object
north = math.Vector2(0.0, 1.0)
norm = lightPos.normalisedCopy()
angle = pmath.degrees(pmath.atan(north.dotProduct(norm))) + 90
angleMotion = motion.basic.RateChangeHeading(angle,
turnSpeed)
# Move that distance forwards
moveMotion = motion.basic.MoveDistance(angle, length, 3)
self.motionManager.setMotion(angleMotion, moveMotion)
def enter(self):
self._orientation = self.ai.data['buoyOrientation']
# Compute where we want to be
xOffset = -self._distance * pmath.sin(self._orientation)
yOffset = self._distance * pmath.cos(self._orientation)
centerBuoy = self.ai.data['buoyList'][1]
buoyObstacle = getObstacleType(centerBuoy)
buoyLocation = self.stateEstimator.getObstacleLocation(buoyObstacle)
xDesired = buoyLocation.x + xOffset
yDesired = buoyLocation.y + yOffset
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue = self.stateEstimator.getEstimatedDepth(),
finalValue = self._depth,
initialRate = self.stateEstimator.getEstimatedDepthRate(),
avgRate = self._diveRate)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = self.stateEstimator.getEstimatedPosition(),
finalValue = math.Vector2(xDesired, yDesired),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._speed)
self._orientation = self.ai.data['buoyOrientation']
centerBuoy = self.ai.data['buoyOrder'][1]
buoyObstacle = getObstacleType(centerBuoy)
def enter(self):
self._orientation = self.ai.data['config'].get('windowOrientation', -1)
# Compute where we want to be
xOffset = -self._distance * pmath.sin(self._orientation)
yOffset = self._distance * pmath.cos(self._orientation)
heartSize = self.ai.data['config'].get('heartSize', 'large')
windowObstacle = getObstacleType(heartSize)
windowLocation = self.stateEstimator.getObstacleLocation(
windowObstacle)
xDesired = windowLocation.x + xOffset
yDesired = windowLocation.y + yOffset
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue=self.stateEstimator.getEstimatedDepth(),
finalValue=self.ai.data['config'].get('windowDepth', -1),
initialRate=self.stateEstimator.getEstimatedDepthRate(),
avgRate=self._diveRate)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=self.stateEstimator.getEstimatedPosition(),
finalValue=math.Vector2(yDesired, xDesired),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
# Dive and translate
diveMotion = motion.basic.ChangeDepth(trajectory=diveTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame=Frame.GLOBAL)
self.motionManager.setMotion(diveMotion, translateMotion)
def BIN_FOUND(self, event):
HoveringState.BIN_FOUND(self, event)
# Fire event if we are centered over the bin
if self._currentBin(event):
if math.Vector2(event.x, event.y).length() < self._centeredRange:
self.publish(BinSortingState.CENTERED_, core.Event())
def enter(self):
if(self.ai.data['fakeGate']):
self._distance = self.ai.data['fakeGateDistance']
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = math.Quaternion(
math.Degree(self.ai.data['gateOrientation']), math.Vector3.UNIT_Z),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
forwardTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = math.Vector2.ZERO,
finalValue = math.Vector2(self._distance,0),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._avgRate)
forwardMotion = motion.basic.Translate(
trajectory = forwardTrajectory,
frame = Frame.LOCAL)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
# Full speed ahead!!
self.motionManager.setMotion(yawMotion, forwardMotion)
def enter(self):
self._className = type(self).__name__
taskTimeout = self.ai.data['config'].get(self._className, {}).get(
'taskTimeout', 30)
Xpos = self.ai.data['config'].get(self._className, {}).get(
'X', 0)
Ypos = self.ai.data['config'].get(self._className, {}).get(
'Y', 0)
yaw = self.ai.data['config'].get(self._className, {}).get(
'orientation', 0)
speed = self.ai.data['config'].get(self._className, {}).get(
'speed', 0.2)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation, yaw),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = math.Vector2.ZERO,
finalValue = math.Vector2(Ypos,Xpos),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = speed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
def RETURN_(self, event):
location = math.Vector2(self.ai.data['lastBinX'], self.ai.data['lastBinY'])
foundId = None
foundIdDistance = None
binData = self.ai.data['binData']
currentBins = [b for b in binData['currentIds']]
for id in currentBins:
binLocation = math.Vector2(binData['itemData'][id].x,
binData['itemData'][id].y)
if foundId is None:
foundId = id
foundIdDistance = (binLocation - location).length()
else:
if (binLocation - location).length() < foundIdDistance:
foundId = id
foundIdDistance = (binLocation - location).length()
self.ai.data['binData']['currentID'] = foundId
def _compareByDistance(self, idA, idB):
"""
Sorts the list with the closest bin at the start
@type idA: int
@param idA: ID of the bin compare
@type idB: int
@param idB: ID of the other bin to compare
"""
binData = self.ai.data['binData']['itemData']
binADistance = math.Vector2(binData[idA].x, binData[idA].y).length()
binBDistance = math.Vector2(binData[idB].x, binData[idB].y).length()
if binADistance < binBDistance:
return -1
else:
return 1
def __init__(self, config, eventHub, vehicle):
self._name = config['name']
SimDevice.__init__(self)
device.IPositionSensor.__init__(self, eventHub, self._name)
simDevice = vehicle.getDevice('SimulationDevice')
self.robot = simDevice.robot
self.initialPos = math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
def enter(self):
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(-self._distance, 0),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
translateMotion = motion.basic.Translate(
trajectory=translateTrajectory, frame=Frame.LOCAL)
self.motionManager.setMotion(translateMotion)
def BIN_FOUND(self, event):
eventDistance = math.Vector2(event.x, event.y).length()
closestIdEvent = self._findClosestBinIdEvent()
if closestIdEvent is None:
self.ai.data['binData']['currentID'] = event.id
else:
closestIdDistance = math.Vector2(closestIdEvent.x,
closestIdEvent.y).length()
if closestIdDistance < eventDistance:
self.ai.data['binData']['currentID'] = closestIdEvent.id
else:
self.ai.data['binData']['currentID'] = event.id
HoveringState.BIN_FOUND(self, event)
if self._currentBin(event):
if eventDistance < self._centeredLimit:
self.publish(Seeking.BIN_CENTERED, core.Event())
def _setvelocity(self):
"""
Given the current desired speeds, calcualte what our inertial
frame velocity should be and send the appropriate command to
the controller.
"""
pos = self._estimator.getEstimatedPosition()
yaw = self._estimator.getEstimatedOrientation().getYaw()
nRb = math.Matrix2.nRb(yaw)
self._controller.translate(
pos, nRb * math.Vector2(self._speed, self._tspeed))
def __init__(self, cfg, deps):
estimation.IStateEstimator.__init__(
self, cfg.get('name', 'StateEstimator'),
core.Subsystem.getSubsystemOfType(core.EventHub, deps))
self._vehicle = core.Subsystem.getSubsystemOfType(
vehicle.IVehicle, deps)
# Must have simulation device to use this estimator
if self._vehicle is not None and \
'SimulationDevice' in self._vehicle.getDeviceNames():
simDevice = self._vehicle.getDevice('SimulationDevice')
self.robot = simDevice.robot
else:
# Print out a warning and continue
print 'Warning! No simulation device found.'
self.robot = None
self._obstacles = {}
# For the position
self.initialPos = math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
self.initialDepth = -3.281 * self.robot._main_part._node.position.z
self.position = self.initialPos
self.velocity = math.Vector2(0, 0)
self.depth = -3.281 * self.robot._main_part._node.position.z
self.depthRate = 0
self.depthFilter = math.SGolaySmoothingFilter(101, 3)
for n in range(1,101):
self.depthFilter.addValue(self.depth)
self.posXFilter = math.SGolaySmoothingFilter(51,2)
for n in range(1,51):
self.posXFilter.addValue(self.initialPos[0])
self.posYFilter = math.SGolaySmoothingFilter(51,2)
for n in range(1,51):
self.posYFilter.addValue(self.initialPos[1])
def enter(self):
forwardTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(self._distance, 0),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._avgRate)
forwardMotion = motion.basic.Translate(trajectory=forwardTrajectory,
frame=Frame.LOCAL)
# Full speed ahead!!
self.motionManager.setMotion(forwardMotion)
def testTranslate(self):
# this doesnt need to test how we get to the result, it only
# tests that we get there without crashing along the way
m = motion.basic.Translate(StepTrajectory(
initialValue=math.Vector2(10, 10),
finalValue=math.Vector2(5, 5),
initialRate=math.Vector2(0, 0),
finalRate=math.Vector2(0, 0)),
updateRate=1)
self.estimator.position = math.Vector2(5, 5)
self.estimator.velocity = math.Vector2(0, 0)
self.qeventHub.subscribeToType(motion.basic.Motion.FINISHED,
self.handleFinished)
# set the motion
self.motionManager.setMotion(m)
# publish the event to update
timer = support.MockTimer.LOG[
motion.basic.Translate.INPLANE_TRAJECTORY_UPDATE]
timer.finish()
self.qeventHub.publishEvents()
# see if the motion finished
self.assert_(self.motionFinished)
# Make sure we have reached the final value
self.assert_(self.controller.atPosition())
def testOffset(self):
# Vehicle pointed 30 degrees left
self.vehicle.orientation = math.Quaternion(math.Degree(30),
math.Vector3.UNIT_Z)
self.vehicle.position = math.Vector2(0, 0)
# Move in a direction 45 degrees left
m = self.makeClass(desiredHeading=75, speed=5, distance=5)
self.motionManager.setMotion(m)
expectedSpeed = pmath.sqrt(2) / 2.0 * 5
self.assertAlmostEqual(expectedSpeed, self.controller.speed, 4)
self.assertAlmostEqual(-expectedSpeed, self.controller.sidewaysSpeed,
4)
def update(self, time):
# On each update, we'll find the velocity using the old position
currentPos = math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
# Subtract by the old position and divide by time
self.velocity = (currentPos - self.oldPos) / time
# Update the position
self.oldPos = currentPos
# Publish an update event
event = math.Vector2Event()
event.vector2 = self.getVelocity()
self.publish(device.IVelocitySensor.UPDATE, event)
def testPositionUpdate(self):
""" Make sure the motion only finishes on an update at the target """
self.vehicle.position = math.Vector2(0, 0)
self.vehicle.orientation = math.Quaternion.IDENTITY
m = self.makeClass(desiredHeading=-90, speed=6, distance=5)
self.motionManager.setMotion(m)
self.assertAlmostEqual(0, self.controller.speed, 5)
self.assertAlmostEqual(6, self.controller.sidewaysSpeed, 5)
# Change vehicle position
position = math.Vector2(2.5, 2.5)
self.vehicle.publishPositionUpdate(position)
self.qeventHub.publishEvents()
# Make sure the speeds result from the updated position not the
# starting one
expectedSpeed = pmath.sqrt(2) / 2.0 * 6
self.assertAlmostEqual(-expectedSpeed, self.controller.speed, 6)
self.assertAlmostEqual(expectedSpeed, self.controller.sidewaysSpeed, 6)
self.motionManager.stopCurrentMotion()
def Strafe(self, strafeDirection, firstMove):
#Defines the strafe movement
currentOrientation = self.stateEstimator.getEstimatedOrientation()
forwardTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(
0, (strafeDirection * self._boundingWidth) / firstMove),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._avgRate)
forwardMotion = motion.basic.Translate(trajectory=forwardTrajectory,
frame=Frame.LOCAL)
self.motionManager.setMotion(forwardMotion)
def move(self, distance):
if not self.move_again:
return
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(0, distance),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
translateMotion = motion.basic.Translate(
trajectory=translateTrajectory, frame=Frame.LOCAL)
self.motionManager.setMotion(translateMotion)
self.move_again = False
def computeValue(self, time):
# compute the time from the beginning of the trajectory
# because we have constructed the trajectory starting at time 0
ts = time - self._initialTime
if ts < 0:
return self._initialValue
elif ts >= self._timePeriod:
return self._finalValue
else:
c = self._coefficients
valueScalar = c[0] + ts * (c[1] + ts * (c[2] + ts * c[3]))
v = self._projectOntoAxes(valueScalar)
v0 = v[0] + self._initialValue[0]
v1 = v[1] + self._initialValue[1]
return math.Vector2(v0, v1)
def update(self):
if (self.RANGE < self._range_thresh):
self.publish(Attack.DONE, core.Event())
else:
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=math.Vector2.ZERO,
finalValue=math.Vector2(self._distance,
self._correct_factor * self.X),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
translateMotion = motion.basic.Translate(
trajectory=translateTrajectory, frame=Frame.LOCAL)
self.motionManager.setMotion(translateMotion)
