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
#
# 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
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
