Source code for test_mrpFeedback

#
#  ISC License
#
#  Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
#  Permission to use, copy, modify, and/or distribute this software for any
#  purpose with or without fee is hereby granted, provided that the above
#  copyright notice and this permission notice appear in all copies.
#
#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#
#
#   Unit Test Script
#   Module Name:        mrpFeedback
#   Author:             Hanspeter Schaub
#   Creation Date:      December 18, 2015
#
# import packages as needed e.g. 'numpy', 'ctypes, 'math' etc.
import numpy as np
import pytest
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import mrpFeedback
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport


#   Import all of the modules that we are going to call in this simulation

# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
#@pytest.mark.xfail()
# provide a unique test method name, starting with test_


[docs] @pytest.mark.parametrize("intGain", [0.01, -1]) @pytest.mark.parametrize("rwNum", [4, 0]) @pytest.mark.parametrize("integralLimit", [0, 20]) @pytest.mark.parametrize("ctrlLaw", [0, 1]) @pytest.mark.parametrize("useRwAvailability", ["NO", "ON", "OFF"]) def test_MRP_Feedback(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): r""" **Validation Test Description** The unit test for this module tests a set of gains :math:`K`, :math:`K_i`, :math:`P` on a rigid body with no external torques, and with a fixed input reference attitude message. The torque requested by the controller is evaluated against python computed torques at 0s, 0.5s, 1s, 1.5s and 2s to within a tolerance of :math:`10^{-8}`. After 1s the simulation is stopped and the ``Reset()`` function is called to check that integral feedback related variables are properly reset. The following permutations are run: - The test is run for a case with error integration feedback (:math:`k_i`=0.01) and one case where :math:`k_i` is set to a negative value, resulting in a case with no integrator. - The RW array number is configured either to 4 or 0 - The integral limit term is set to either 0 or 20 - The RW availability message is tested in 3 manners. Either the availability message is not written where all wheels should default to being available. If the availability message is written, then the RWs are either zero to available or not available. - The control parameter :math:`\delta\omega_{0}` is set to either a zero or non-zero vector All permutations of these test cases are expected to pass. **Test Parameters** Args: intGain (float): value of the integral gain :math:`K_i` rwNum (int): number of RW devices to simulate integralLimit (float): value of the integral limit ctrlLaw (int): type of control law used useRwAvailability (string): Flag to not use RW availabillity (``NO``), use the availability message and turn on the RW devices (``ON``) and use the message and turn off the devices (``OFF``) """ # each test method requires a single assert method to be called [testResults, testMessage] = run(show_plots,intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability) assert testResults < 1, testMessage
def run(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # this creates a fresh and consistent simulation environment for each test run # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = mrpFeedback.mrpFeedback() module.ModelTag = "mrpFeedback" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data module.K = 0.15 module.Ki = intGain module.P = 150.0 module.integralLimit = integralLimit module.controlLawType = ctrlLaw module.knownTorquePntB_B = [1., 1., 1.] # create input messages # AttGuidFswMsg Message: guidCmdData = messaging.AttGuidMsgPayload() sigma_BR = [0.3, -0.5, 0.7] guidCmdData.sigma_BR = sigma_BR omega_BR_B = [0.010, -0.020, 0.015] guidCmdData.omega_BR_B = omega_BR_B omega_RN_B = [-0.02, -0.01, 0.005] guidCmdData.omega_RN_B = omega_RN_B domega_RN_B = [0.0002, 0.0003, 0.0001] guidCmdData.domega_RN_B = domega_RN_B guidInMsg = messaging.AttGuidMsg().write(guidCmdData) # vehicleConfigData Message: vehicleConfig = messaging.VehicleConfigMsgPayload() I = [1000., 0., 0., 0., 800., 0., 0., 0., 800.] vehicleConfig.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfig) # wheelSpeeds Message rwSpeedMessage = messaging.RWSpeedMsgPayload() Omega = [10.0, 25.0, 50.0, 100.0] # rad/sec rwSpeedMessage.wheelSpeeds = Omega rwSpeedInMsg = messaging.RWSpeedMsg().write(rwSpeedMessage) # wheelConfigData message jsList = [] GsMatrix_B = [] def writeMsgInWheelConfiguration(): rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.577350269190, 0.577350269190, 0.577350269190 ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = rwNum msg = messaging.RWArrayConfigMsg().write(rwConfigParams) return rwConfigParams.JsList, rwConfigParams.GsMatrix_B, msg if rwNum > 0: jsList, GsMatrix_B, rwParamInMsg = writeMsgInWheelConfiguration() # wheelAvailability message rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() if useRwAvailability != "NO": if useRwAvailability == "ON": rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE] elif useRwAvailability == "OFF": rwAvailabilityMessage.wheelAvailability = [messaging.UNAVAILABLE, messaging.UNAVAILABLE, messaging.UNAVAILABLE, messaging.UNAVAILABLE] else: print("WARNING: unknown rw availability status") rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) else: # set default availability rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE] LrTrue = findTrueTorques(module, guidCmdData, rwSpeedMessage, vehicleConfig, jsList, rwNum, GsMatrix_B, rwAvailabilityMessage, ctrlLaw) # Setup logging on the test module output message so that we get all the writes to it dataLog = module.cmdTorqueOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # connect messages module.guidInMsg.subscribeTo(guidInMsg) module.vehConfigInMsg.subscribeTo(vcInMsg) if rwNum > 0: module.rwParamsInMsg.subscribeTo(rwParamInMsg) module.rwSpeedsInMsg.subscribeTo(rwSpeedInMsg) if useRwAvailability != "NO": module.rwAvailInMsg.subscribeTo(rwAvailInMsg) # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() # Step the simulation to 3*process rate so 4 total steps including zero unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() module.Reset(1) # this module reset function needs a time input (in NanoSeconds) unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # compare the module results to the truth values accuracy = 1e-8 for i in range(0, len(LrTrue)): # check vector values if not unitTestSupport.isArrayEqual(dataLog.torqueRequestBody[i], LrTrue[i], 3, accuracy): testFailCount += 1 testMessages.append("FAILED: " + module.ModelTag + " Module failed mrpFeedback unit test at t=" + str(dataLog.times()[i]*macros.NANO2SEC) + "sec\n") # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag) else: print("Failed: " + module.ModelTag) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found return [testFailCount, ''.join(testMessages)] def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,numRW,GsMatrix_B,rwAvailMsg,ctrlLaw): Lr = [] #Read in variables L = np.asarray(module.knownTorquePntB_B) steps = [0, 0, .5, 0, .5] omega_BR_B = np.asarray(guidCmdData.omega_BR_B) omega_RN_B = np.asarray(guidCmdData.omega_RN_B) omega_BN_B = omega_BR_B + omega_RN_B #find body rate domega_RN_B = np.asarray(guidCmdData.domega_RN_B) sigma_BR = np.asarray(guidCmdData.sigma_BR) Isc = np.asarray(vehicleConfigOut.ISCPntB_B) Isc = np.reshape(Isc, (3, 3)) Ki = module.Ki K = module.K P = module.P jsVec = jsList GsMatrix_B_array = np.asarray(GsMatrix_B) GsMatrix_B_array = np.reshape(GsMatrix_B_array[0:numRW * 3], (numRW, 3)) sigmaInt = np.asarray([0, 0, 0]) #Compute toruqes for i in range(len(steps)): dt = steps[i] if dt == 0: sigmaInt = np.asarray([0, 0, 0]) #evaluate integral term if Ki > 0: #if integral feedback is on sigmaInt = K * dt * sigma_BR + sigmaInt for n in range(3): if abs(sigmaInt[n]) > module.integralLimit: sigmaInt[n] *= module.integralLimit/sigmaInt[n] #check elementwise if integral term is greater than limit; preserve direction (+/-) zVec = sigmaInt + Isc.dot(omega_BR_B) else: #integral gain turned off/negative setting zVec = np.asarray([0, 0, 0]) #compute torque Lr Lr0 = K * sigma_BR # +K*sigmaBR Lr1 = Lr0 + P * omega_BR_B # +P*deltaOmega Lr2 = Lr1 + P * Ki * zVec # +P*Ki*z GsHs = np.array([0,0,0]) if numRW>0: for i in range(numRW): if rwAvailMsg.wheelAvailability[i] == 0: #Make RW availability check GsHs = GsHs + np.dot(GsMatrix_B_array[i, :], jsVec[i]*(np.dot(omega_BN_B, GsMatrix_B_array[i, :])+rwSpeedMessage.wheelSpeeds[i])) #J_s*(dot(omegaBN_B,Gs_vec)+Omega_wheel) if ctrlLaw == 0: Lr3 = Lr2 - np.cross((omega_RN_B+Ki*zVec), (Isc.dot(omega_BN_B)+GsHs)) # -[v3Tilde(omega_r+Ki*z)]([I]omega + [Gs]h_s) else: Lr3 = Lr2 - np.cross(omega_BN_B, (Isc.dot(omega_BN_B)+GsHs)) # -[v3Tilde(omega)]([I]omega + [Gs]h_s) Lr4 = Lr3 + Isc.dot(-domega_RN_B + np.cross(omega_BN_B, omega_RN_B)) #+[I](-d(omega_r)/dt + omega x omega_r) Lr5 = Lr4 + L Lr5 = -Lr5 Lr.append(np.ndarray.tolist(Lr5)) return Lr # This statement below ensures that the unitTestScript can be run as a stand-along python scripts # automatically executes the test_MRP_Feedback() method # if __name__ == "__main__": test_MRP_Feedback(False, # showplots 0.01, # intGain 0, # rwNum 0.0, # integralLimit "NO" # useRwAvailability ("NO", "ON", "OFF") )