def changedPipe(self):
# A stupid solution. Stop current motion and restart it when
# PipeInfo is changed
if not self._changeMotion(self._currentDesiredPos, ext.math.Vector2(
self._pipe.getY() * self._forwardRate,
self._pipe.getX() * self._sidewaysRate)):
return
self._currentDesiredPos = ext.math.Vector2(self._pipe.getY() *
self._forwardRate,
self._pipe.getX() *
self._sidewaysRate)
self.motionManager.stopCurrentMotion()
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation, self._pipe.getAngle()),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = self._currentDesiredPos,
initialRate = ext.math.Vector2.ZERO,
avgRate = self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
def enter(self):
PipeTrackingState.enter(self)
state.FindAttempt.enter(self)
self.visionSystem.pipeLineDetectorOn()
event = self.ai.data.get('lastPipeEvent', None)
if event is not None:
ahead = ext.math.Vector3(event.x, event.y, 0)
quat = ext.math.Vector3.UNIT_Y.getRotationTo(ahead)
self._direction = quat.getYaw(True)
self._speed = self._config.get('speed', 0.15)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation,
self._direction.valueDegrees()),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = ext.math.Vector2(5,0),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._speed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
#self.motionManager.setMotion(translateMotion)
else:
self.motionManager.stopCurrentMotion()
def enter(self, angle = 0):
self._vase = VaseInfo(0,0,0)
self._angleOffset = angle
self._currentDesiredPos = ext.math.Vector2(self._vase.getY() *
self._forwardRate,
self._vase.getX() *
self._sidewaysRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation,
self._vase.getAngle()),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = self._currentDesiredPos,
initialRate = ext.math.Vector2.ZERO,
avgRate = self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
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 enter(self):
PipeTrackingState.enter(self)
state.FindAttempt.enter(self)
self.visionSystem.pipeLineDetectorOn()
event = self.ai.data.get('lastPipeEvent', None)
if event is not None:
ahead = ext.math.Vector3(event.x, event.y, 0)
quat = ext.math.Vector3.UNIT_Y.getRotationTo(ahead)
self._direction = quat.getYaw(True)
self._speed = self._config.get('speed', 0.15)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue=currentOrientation,
finalValue=yawVehicleHelper(currentOrientation,
self._direction.valueDegrees()),
initialRate=self.stateEstimator.getEstimatedAngularRate(),
finalRate=ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=ext.math.Vector2.ZERO,
finalValue=ext.math.Vector2(5, 0),
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._speed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame=Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
#self.motionManager.setMotion(translateMotion)
else:
self.motionManager.stopCurrentMotion()
def changedPipe(self):
# A stupid solution. Stop current motion and restart it when
# PipeInfo is changed
if not self._changeMotion(
self._currentDesiredPos,
ext.math.Vector2(self._pipe.getY() * self._forwardRate,
self._pipe.getX() * self._sidewaysRate)):
return
self._currentDesiredPos = ext.math.Vector2(
self._pipe.getY() * self._forwardRate,
self._pipe.getX() * self._sidewaysRate)
self.motionManager.stopCurrentMotion()
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue=currentOrientation,
finalValue=yawVehicleHelper(currentOrientation,
self._pipe.getAngle()),
initialRate=self.stateEstimator.getEstimatedAngularRate(),
finalRate=ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=ext.math.Vector2.ZERO,
finalValue=self._currentDesiredPos,
initialRate=ext.math.Vector2.ZERO,
avgRate=self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame=Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
def PIPE_FOUND(self,event):
if((abs(event.angle)-90) < self._yaw_bound):
self.publish(DONE,core.Event())
else:
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation,
-event.angle),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(yawMotion)
def yaw(self,deg,t):
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation,
deg),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(yawMotion)
self.timer = self.timerManager.newTimer(YAWED, self._delay)
self.timer.start()
self._mYaw = True
def UPDATE(self, event):
PingerState.UPDATE(self, event)
if self._isNewPing(event):
# Converting from the vehicle reference frame, to the image space
# reference frame used by the pipe motion
self._pipe.setState(-event.direction.y, event.direction.x,
ext.math.Degree(0), event.timeStamp)
if not self._changeMotion(self._currentDesiredPos,
ext.math.Vector2(self._pipe.getY() *
self._distance,
self._pipe.getX() *
self._distance)) or self._recalculate:
self._recalculate = False
return
self._currentDesiredPos = ext.math.Vector2(self._pipe.getY() *
self._distance,
self._pipe.getX() *
self._distance)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
orientation = yawVehicleHelper(currentOrientation,
self._pipe.getAngle())
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = orientation,
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = self._currentDesiredPos,
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = 0.15)
#self.motionManager.stopCurrentMotion()
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
if math.fabs(event.direction.z) > math.fabs(self._closeZ):
self.publish(TranslationSeeking.CLOSE, core.Event())
def enter(self):
self._ignoreEvents = False
PipeTrackingState.enter(self)
self._pipe = PipeInfo(0, 0, 0)
speedGain = self._config.get('speedGain', 7)
dSpeedGain = self._config.get('dSpeedGain', 1)
iSpeedGain = self._config.get('iSpeedGain', 0.1)
sidewaysSpeedGain = self._config.get('sidewaysSpeedGain', 3)
iSidewaysSpeedGain = self._config.get('iSidewaysSpeedGain', 0.05)
dSidewaysSpeedGain = self._config.get('dSidewaysSpeedGain', 0.5)
yawGain = self._config.get('yawGain', 0.3)
self._forwardSpeed = self._config.get('forwardSpeed', 0.15)
maxSidewaysSpeed = self._config.get('maxSidewaysSpeed', 3)
self._forwardRate = self._config.get('forwardRate', 1)
self._sidewaysRate = self._config.get('sidewaysRate', 1)
self._motionRange = self._config.get('motionRange', .1)
self._currentDesiredPos = ext.math.Vector2(self._pipe.getY() *
self._forwardRate,
self._pipe.getX() *
self._sidewaysRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation, self._pipe.getAngle()),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = self._currentDesiredPos,
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
def enter(self):
self._ignoreEvents = False
PipeTrackingState.enter(self)
self._pipe = PipeInfo(0, 0, 0)
speedGain = self._config.get('speedGain', 7)
dSpeedGain = self._config.get('dSpeedGain', 1)
iSpeedGain = self._config.get('iSpeedGain', 0.1)
sidewaysSpeedGain = self._config.get('sidewaysSpeedGain', 3)
iSidewaysSpeedGain = self._config.get('iSidewaysSpeedGain', 0.05)
dSidewaysSpeedGain = self._config.get('dSidewaysSpeedGain', 0.5)
yawGain = self._config.get('yawGain', 0.3)
self._forwardSpeed = self._config.get('forwardSpeed', 0.15)
maxSidewaysSpeed = self._config.get('maxSidewaysSpeed', 3)
self._forwardRate = self._config.get('forwardRate', 1)
self._sidewaysRate = self._config.get('sidewaysRate', 1)
self._motionRange = self._config.get('motionRange', .1)
self._currentDesiredPos = ext.math.Vector2(
self._pipe.getY() * self._forwardRate,
self._pipe.getX() * self._sidewaysRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue=currentOrientation,
finalValue=yawVehicleHelper(currentOrientation,
self._pipe.getAngle()),
initialRate=self.stateEstimator.getEstimatedAngularRate(),
finalRate=ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue=ext.math.Vector2.ZERO,
finalValue=self._currentDesiredPos,
initialRate=self.stateEstimator.getEstimatedVelocity(),
avgRate=self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame=Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
def enter(self, timeout = 2.5):
self._pipe = ram.motion.pipe.Pipe(0, 0, 0, timeStamp = None)
self._lastTime = 0
self._recalculate = False
self._loadSettings()
self._currentDesiredPos = ext.math.Vector2(self._pipe.getY() *
self._distance,
self._pipe.getX() *
self._distance)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
orientation = yawVehicleHelper(currentOrientation,
self._pipe.getAngle())
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = orientation,
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = ext.math.Vector3.ZERO)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = ext.math.Vector2.ZERO,
finalValue = self._currentDesiredPos,
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._forwardSpeed)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(yawMotion, translateMotion)
# Set up the ping timer
self._pingChecker = None
self._timeout = self._config.get('timeout', timeout)
if self._timeout > 0:
self._pingChecker = self.timerManager.newTimer(
PingerState.TIMEOUT, self._timeout)
self._pingChecker.start()
def enter(self):
self.visionSystem.loversLaneDetectorOn()
depth = self.ai.data['laneDepth']
orientation = self.ai.data['laneOrientation']
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue = self.stateEstimator.getEstimatedDepth(),
finalValue = depth,
initialRate = self.stateEstimator.getEstimatedDepthRate(),
avgRate = self._diveRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = yawVehicleHelper(currentOrientation, orientation),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
# Dive and translate
diveMotion = motion.basic.ChangeDepth(trajectory = diveTrajectory)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(yawMotion, diveMotion)
def enter(self):
self.visionSystem.loversLaneDetectorOn()
depth = self.ai.data['laneDepth']
orientation = self.ai.data['laneOrientation']
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue=self.stateEstimator.getEstimatedDepth(),
finalValue=depth,
initialRate=self.stateEstimator.getEstimatedDepthRate(),
avgRate=self._diveRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue=currentOrientation,
finalValue=yawVehicleHelper(currentOrientation, orientation),
initialRate=self.stateEstimator.getEstimatedAngularRate(),
finalRate=math.Vector3.ZERO)
# Dive and translate
diveMotion = motion.basic.ChangeDepth(trajectory=diveTrajectory)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(yawMotion, diveMotion)
