def makeNmpc(propertiesDir='../properties'): conf = makeConf() conf['stabilize_invariants'] = False dae = carouselModel.makeModel(conf,propertiesDir=propertiesDir) mpc = rawe.Mpc(dae, N=mpcHorizonN, ts=Ts) # mpc.constrain( mpc['dddr'], '==', 0 ); mpc.constrain( radians(-5.0), '<=', mpc['aileron'], '<=', radians(5.0) ); mpc.constrain( radians(-5.0), '<=', mpc['elevator'], '<=', radians(5.0) ); mpc.constrain( -0.2*100, '<=', mpc['daileron'], '<=', 0.2*100 ); mpc.constrain( -1*100, '<=', mpc['delevator'], '<=', 1*100 ); mpc.constrain( 0, '<=', mpc['motor_torque'], '<=', 2000 ); return mpc
def makeMhe(propertiesDir='../properties'): conf = makeConf() conf['stabilize_invariants'] = False dae = carouselModel.makeModel(conf,propertiesDir=propertiesDir) mhe = rawe.Mhe(dae, N=mheHorizonN, ts=Ts, yxNames=measX, yuNames=measU) mhe.constrain(mhe['ConstR1'],'==',0, when='AT_START') mhe.constrain(mhe['ConstR2'],'==',0, when='AT_START') mhe.constrain(mhe['ConstR3'],'==',0, when='AT_START') mhe.constrain(mhe['ConstR4'],'==',0, when='AT_START') mhe.constrain(mhe['ConstR5'],'==',0, when='AT_START') mhe.constrain(mhe['ConstR6'],'==',0, when='AT_START') mhe.constrain(mhe['c'],'==',0, when='AT_START') mhe.constrain(mhe['cdot'],'==',0, when='AT_START') mhe.constrain(mhe['ConstDelta'],'==',0, when='AT_START') return mhe
def makeMhe(Ts = None, propertiesDir = '../../properties'): # # Make the parameter configuration for the model # conf = makeConf() conf[ 'stabilize_invariants' ] = False if useVirtualTorques is True: conf[ 'useVirtualTorques' ] = True if useVirtualForces is True: conf[ 'useVirtualForces' ] = True # # Create the DAE # dae = carouselModel.makeModel(conf, propertiesDir = propertiesDir) if Ts is None: execSamplingTime = samplingTime else: execSamplingTime = Ts mhe = rawe.Mhe(dae, N = mheHorizonN, ts = execSamplingTime, yxNames = measX, yuNames = measU) # mheConstraintsWhen = 'AT_START' mheConstraintsWhen = 'AT_END' mhe.constrain(mhe['ConstR1'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstR2'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstR3'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstR4'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstR5'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstR6'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['c'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['cdot'], '==', 0, when = mheConstraintsWhen) mhe.constrain(mhe['ConstDelta'], '==', 0, when = mheConstraintsWhen) return mhe, conf
except ImportError: useProtobufBridge = False from mpc_mhe_utils import Plotter from rawe.models.arianne_conf import makeConf from rawekite.carouselSteadyState import getSteadyState # This is the new one, used in experiments. # from offline_mhe_test import carouselModel # This is the old one, created by Mario, Greg and friends. import carouselModel from common_conf import Ts conf = makeConf() conf['stabilize_invariants'] = True daeSim = carouselModel.makeModel(conf) # Create the MPC class mheRT = MHE.makeMheRT() mpcRT = NMPC.makeNmpcRT(daeSim) mheSigmas = {'cos_delta':1e-1, 'sin_delta':1e-1, 'IMU_angular_velocity':1.0, 'IMU_acceleration':10.0, 'marker_positions':1e2, 'r':1e-1, 'aileron':1e-2, 'elevator':1e-2, 'daileron':1e-4,
# # You should have received a copy of the GNU Lesser General Public License # along with rawesome. If not, see <http://www.gnu.org/licenses/>. import casadi as C import time import rawe import rawekite from rawekite.carouselSteadyState import getSteadyState if __name__=='__main__': # create the model from rawe.models.arianne_conf import makeConf conf = makeConf() dae = rawe.models.carousel(makeConf()) # compute the steady state steadyState, ssDot = getSteadyState(dae,conf,2*C.pi,1.2,0.1) print steadyState print ssDot # create the sim dt = 0.01 sim = rawe.sim.Sim(dae,dt) # communicator = rawekite.communicator.Communicator() # js = joy.Joy() # set the initial state from steadyState
def get_steady_state_guess(): from rawekite.carouselSteadyState import getSteadyState conf_ss = makeConf() steadyState,_ = getSteadyState(carousel_dae.makeDae(conf_ss), None, ddelta0, lineRadiusGuess, {}) return steadyState
yn = y mpcRT.y[:, k] = y mpcRT.yN[k] = yn for k, name in enumerate(mpcRT.ocp.dae.uNames()): mpcRT.y[:, k + nx] = steadyState[name] ############################################################################### # # Get the initial guess for the MHE & NMPC by calculating a steady state # ############################################################################### # Get the plane configuration parameters conf = makeConf() # conf['stabilize_invariants'] = True dae = carouselModel.makeModel(makeConf(), propertiesDir = '../../properties') # Reference parameters # refP = {'r0': 1.275, # [m], cable length used for SS calculations # 'ddelta0': -4.0, # [rad/s], speed used for SS calculations # } refP = {'r0': 2.0, # [m], cable length used for SS calculations 'ddelta0':-5, # [rad/s], speed used for SS calculations } # Get the steady state steadyState, dSS = getSteadyState(dae, conf, refP['ddelta0'], refP['r0'])
def makeDae( conf = None ): if conf is None: conf = arianne_conf.makeConf() # Make model dae = rawe.models.carousel(conf) (xDotSol, zSol) = dae.solveForXDotAndZ() # Get variables and outputs from the model ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']]) ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']]) x = dae['x'] y = dae['y'] z = dae['z'] e31 = dae['e31'] e32 = dae['e32'] e33 = dae['e33'] zt = conf['zt'] dx = dae['dx'] dy = dae['dy'] ddelta = dae['ddelta'] dddelta = xDotSol['ddelta'] R = dae['R_c2b'] rA = conf['rArm'] g = conf['g'] # Rotation matrix to convert from NWU to NED frame type R_nwu2ned = np.eye( 3 ) R_nwu2ned[1, 1] = R_nwu2ned[2, 2] = -1.0 ############################################################################ # # IMU model # ############################################################################ # Load IMU position and orientation w.r.t. body frame # pIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat')))) pIMU = C.DMatrix([0,0,0]) pIMU = C.mul(R_nwu2ned, pIMU) #0 #0 #0 # RIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat')))) #9.937680e-01 6.949103e-02 8.715574e-02 #-6.975647e-02 9.975641e-01 0 #-8.694344e-02 -6.079677e-03 9.961947e-01 RIMU = C.DMatrix([[9.937680e-01, 6.949103e-02, 8.715574e-02], [-6.975647e-02, 9.975641e-01, 0], [-8.694344e-02, -6.079677e-03, 9.961947e-01]]) RIMU = C.mul(R_nwu2ned, RIMU) # Define IMU measurement functions # TODO here is omitted the term: w x w pIMU # The sign of gravity is negative because of the NED convention (z points down!) ddpIMU_c = ddp - ddelta ** 2 * C.vertcat([x + rA, y, 0]) + 2 * ddelta * C.vertcat([-dy, dx, 0]) + \ dddelta * C.vertcat([-y, x + rA, 0]) - C.vertcat([0, 0, g]) ddpIMU = C.mul(R, ddpIMU_c) aBridle = C.cross(ddt_w_bn_b, pIMU) ddpIMU += aBridle ddpIMU = C.mul(RIMU,ddpIMU) # You can add a parameter to conf which will give the model 3 extra states with derivative 0 (to act as parameter) for the bias in the acceleration measurements. If that is present, it should be added to the measurement of the acceleration if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']]) ddpIMU += IMUAccelerationBias # For the accelerometers dae['IMU_acceleration'] = ddpIMU dae['IMU_acceleration_x'] = dae['IMU_acceleration'][0] dae['IMU_acceleration_y'] = dae['IMU_acceleration'][1] dae['IMU_acceleration_z'] = dae['IMU_acceleration'][2] # ... and for the gyroscopes dae['IMU_angular_velocity'] = C.mul(RIMU, dae['w_bn_b']) dae['IMU_angular_velocity_x'] = dae['IMU_angular_velocity'][0] dae['IMU_angular_velocity_y'] = dae['IMU_angular_velocity'][1] dae['IMU_angular_velocity_z'] = dae['IMU_angular_velocity'][2] if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: dae['vel_error_x'] = dae['dx']-dae['dx_IMU'] dae['vel_error_y'] = dae['dy']-dae['dy_IMU'] dae['vel_error_z'] = dae['dz']-dae['dz_IMU'] ############################################################################ # # LAS # ############################################################################ x_tether = (x + zt*e31) y_tether = (y + zt*e32) z_tether = (z + zt*e33) dae['lineAngles'] = C.vertcat([C.arctan(y_tether/x_tether),C.arctan(z_tether/x_tether)]) dae['lineAngle_hor'] = dae['lineAngles'][0] dae['lineAngle_ver'] = dae['lineAngles'][1] return dae