class LQGController: controlForce = 0.0 def __init__(self, inertia, damping, stiffness, timeStep): self.dt = timeStep self.observer = KalmanFilter(3, 3, 1) self.observer.SetMeasurement(0, 0, 1.0) self.observer.SetMeasurement(1, 1, 1.0) self.observer.SetMeasurement(2, 2, 1.0) self.observer.SetStatePredictionFactor(0, 1, self.dt) self.observer.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2) self.observer.SetStatePredictionFactor(1, 2, self.dt) self.observer.SetStatePredictionFactor(2, 2, 0.0) self.SetSystem(inertia, damping, stiffness) def SetSystem(self, inertia, damping, stiffness): A = [[1, self.dt, 0.5 * self.dt**2], [0, 1, self.dt], [-stiffness / inertia, -damping / inertia, 0]] B = [[0], [0], [1 / inertia]] C = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] self.feedbackGain = GetLQRController(A, B, C, 0.0001) self.observer.SetStatePredictionFactor(2, 0, -stiffness / inertia) self.observer.SetStatePredictionFactor(2, 1, -damping / inertia) self.observer.SetInputPredictionFactor(2, 0, 1 / inertia) def Process(self, setpoint, measurement, externalForce): reference = [ measurement[0] - setpoint[0], measurement[1] - setpoint[1], measurement[2] - setpoint[2] ] state, error = self.observer.Process( reference, [self.controlForce + externalForce]) self.controlForce = -self.feedbackGain.dot(state)[0] return self.controlForce
class WavePredTeleoperator: remotePosition = 0.0 outputWaveIntegral = 0.0 def __init__( self, impedance, timeStep ): self.waveController = WaveController( impedance[ 0 ] + impedance[ 1 ] + impedance[ 2 ], timeStep ) self.dt = timeStep self.waveObserver = KalmanFilter( 2, 2 ) self.waveObserver.SetMeasurement( 0, 0, 4.0 ) self.waveObserver.SetMeasurement( 1, 1, 4.0 ) self.waveObserver.SetStatePredictionFactor( 0, 1, self.dt ) def SetSystem( self, impedance ): waveImpedance = ( max( impedance[ 0 ], 0.0 ), max( impedance[ 1 ], 1.0 ), max( impedance[ 2 ], 0.0 ) ) self.waveController.SetImpedance( waveImpedance[ 0 ] + waveImpedance[ 1 ] + waveImpedance[ 2 ] ) def Process( self, localState, localForce, remotePacket, timeDelay ): localPosition, localVelocity, localAcceleration = localState inputWave, inputWaveIntegral, inputEnergy, remoteImpedance = remotePacket remoteState, predictedWave = self.waveObserver.Process( [ inputWaveIntegral + inputWave * timeDelay, inputWave ] ) remoteVelocity = self.waveController.PreProcess( predictedWave[ 1 ], inputEnergy, remoteImpedance ) feedbackForce, outputWave, outputEnergy, localImpedance = self.waveController.PostProcess( localVelocity, localForce ) self.remotePosition += remoteVelocity * self.dt self.outputWaveIntegral += outputWave * self.dt return ( feedbackForce, ( self.remotePosition, remoteVelocity, 0.0 ), ( outputWave, self.outputWaveIntegral, outputEnergy, localImpedance ) )
class KalmanPredictor: state = [0.0, 0.0, 0.0] def __init__(self, timeStep): self.dt = timeStep self.localObserver = KalmanFilter(3, 3) self.localObserver.SetMeasurement(0, 0, 1.0) self.localObserver.SetMeasurement(1, 1, 1.0) self.localObserver.SetMeasurement(2, 2, 1.0) self.localObserver.SetStatePredictionFactor(0, 1, self.dt) self.localObserver.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2) self.localObserver.SetStatePredictionFactor(1, 2, self.dt) self.remoteObserver = KalmanFilter(3, 3) self.remoteObserver.SetMeasurement(0, 0, 1.0) self.remoteObserver.SetMeasurement(1, 1, 1.0) self.remoteObserver.SetMeasurement(2, 2, 1.0) self.remoteObserver.SetPredictionNoise(0, 2.0) self.remoteObserver.SetPredictionNoise(1, 2.0) self.remoteObserver.SetPredictionNoise(2, 2.0) def Process(self, inputPacket, remoteTime, timeDelay): setpoint, setpointVelocity, setpointAcceleration = inputPacket measurement = [setpoint, setpointVelocity, setpointAcceleration] timeDelay = int(timeDelay / self.dt) * self.dt self.remoteObserver.SetStatePredictionFactor(0, 1, timeDelay) self.remoteObserver.SetStatePredictionFactor(0, 2, 0.5 * timeDelay**2) self.remoteObserver.SetStatePredictionFactor(1, 2, timeDelay) newState, prediction = self.remoteObserver.Predict(self.state) self.state, estimatedMeasurement = self.localObserver.Process( measurement) self.remoteObserver.Update(measurement, estimatedMeasurement) self.remoteObserver.Correct() return [prediction[0], prediction[1], prediction[2]]
class LQGPredController: measurement = [0.0, 0.0, 0.0] controlForce = 0.0 def __init__(self, inertia, damping, stiffness, timeStep): self.dt = timeStep self.localController = LQGController(inertia, damping, stiffness, timeStep) self.remoteStateObserver = KalmanFilter(3, 3) self.remoteStateObserver.SetMeasurement(0, 0, 4.0) self.remoteStateObserver.SetMeasurement(1, 1, 4.0) self.remoteStateObserver.SetMeasurement(2, 2, 4.0) self.remoteInputObserver = KalmanFilter(3, 1) self.remoteInputObserver.SetMeasurement(0, 0, 1.0) def SetSystem(self, inertia, damping, stiffness): self.localController.SetSystem(inertia, damping, stiffness) def Predict(self, remoteMeasurement, remoteForce, timeDelay): timeDelay = int(timeDelay / self.dt) * self.dt predictedState = list(remoteMeasurement) predictedState[0] += predictedState[1] * timeDelay + predictedState[ 2] * 0.5 * timeDelay**2 predictedState[1] += predictedState[2] * timeDelay remoteState, predictedState = self.remoteStateObserver.Process( predictedState) remoteState, predictedForce = self.remoteInputObserver.Predict() remoteState, predictedForce = self.remoteInputObserver.Update( [remoteForce], [remoteState[0]]) predictedForce[0] += remoteState[1] * timeDelay + remoteState[ 2] * 0.5 * timeDelay**2 return (tuple(predictedState), predictedForce[0]) def Process(self, setpoint, measurement, externalForce): return self.localController.Process(setpoint, measurement, externalForce)