def pendulumModel(nSteps=None):
dae = Dae()
dae.addZ(["tau"])
dae.addX(["x", "z", "dx", "dz"])
dae.addU(["torque"])
dae.addP(["m"])
r = 0.3
ddx = dae.ddt('dx')
ddz = dae.ddt('dz')
ode = [
dae['dx'] - dae.ddt('x'), dae['dz'] - dae.ddt('z')
# , dae['ddx'] - dae.ddt('dx')
# , dae['ddz'] - dae.ddt('dz')
]
fx = dae['torque'] * dae['z']
fz = -dae['torque'] * dae['x'] + dae['m'] * 9.8
alg = [
dae['m'] * ddx + dae['x'] * dae['tau'] - fx,
dae['m'] * ddz + dae['z'] * dae['tau'] - fz, dae['x'] * ddx +
dae['z'] * ddz + (dae['dx'] * dae['dx'] + dae['dz'] * dae['dz'])
]
dae['c'] = dae['x'] * dae['x'] + dae['z'] * dae['z'] - r * r
dae['cdot'] = dae['dx'] * dae['x'] + dae['dz'] * dae['z']
dae.setResidual([ode, alg])
return dae
def pendulumModel(nSteps=None):
dae = Dae()
dae.addZ( [ "tau"
] )
dae.addX( [ "x"
, "z"
, "dx"
, "dz"
] )
dae.addU( [ "torque"
] )
dae.addP( [ "m"
] )
r = 0.3
ddx = dae.ddt('dx')
ddz = dae.ddt('dz')
ode = [ dae['dx'] - dae.ddt('x')
, dae['dz'] - dae.ddt('z')
# , dae['ddx'] - dae.ddt('dx')
# , dae['ddz'] - dae.ddt('dz')
]
fx = dae['torque']*dae['z']
fz = -dae['torque']*dae['x'] + dae['m']*9.8
alg = [ dae['m']*ddx + dae['x']*dae['tau'] - fx
, dae['m']*ddz + dae['z']*dae['tau'] - fz
, dae['x']*ddx + dae['z']*ddz + (dae['dx']*dae['dx'] + dae['dz']*dae['dz']) ]
dae['c'] = dae['x']*dae['x'] + dae['z']*dae['z'] - r*r
dae['cdot'] = dae['dx']*dae['x'] + dae['dz']*dae['z']
dae.setResidual( [ode, alg] )
return dae
#
# 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 matplotlib.pyplot as plt
from rawe.dae import Dae
from rawe.collocation import Coll
if __name__ == "__main__":
######## make the Dae #######
dae = Dae()
[pos,vel] = dae.addX( ["pos","vel"] )
dae.addX('abspos')
force = dae.addU( "force" )
# some extra outputs for the dae model
dae['force_over_pos'] = force/pos
# specify the ode residual
mass = 1.0
dae.setResidual([dae.ddt('pos') - vel,
dae.ddt('vel') - (force/mass - 3.0*pos - 0.2*vel)]
)
######## make the collocation scheme ########
N = 100
ocp = Coll(dae, nk=N, nicp=1, collPoly="RADAU", deg=4)
endTime = 5
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "dmotor_torque"
, 'dddr'
] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
dae.addP( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
else:
dae.addU( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
import matplotlib.pyplot as plt
from rawe.dae import Dae
from rawe.collocation import Coll
if __name__ == "__main__":
######## make the Dae #######
dae = Dae()
[pos,vel,mass] = dae.addX( ["pos","vel","mass"] )
thrust = dae.addU( "thrust" )
# some extra outputs for the dae model
dae['pos*vel'] = pos*vel
dae['vel*vel'] = vel*vel
# specify the ode residual
dae.setResidual([dae.ddt('pos') - vel,
dae.ddt('vel') - thrust/mass,
dae.ddt('mass') - 0.1*thrust*thrust])
######## make the collocation scheme ########
N = 100
ocp = Coll(dae, nk=N, nicp=1, collPoly="RADAU", deg=4)
endTime = 5
ocp.setupCollocation( endTime )
# bounds
ocp.bound('thrust',(-1.3,0.9))
ocp.bound('pos',(-10,10))
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z',
'e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33',
'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z',
'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag'
])
dae.addU( [ 'daileron'
, 'delevator'
, 'drudder'
, 'dflaps'
, 'dprop_drag'
] )
dae.addP( ['r','w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] )
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul( dae['R_n2b'], v_bw_n )
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = \
aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
# if we are running a homotopy,
# add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1*gamma_homotopy + dae.addZ('f1_homotopy')*(1 - gamma_homotopy)
f2 = f2*gamma_homotopy + dae.addZ('f2_homotopy')*(1 - gamma_homotopy)
f3 = f3*gamma_homotopy + dae.addZ('f3_homotopy')*(1 - gamma_homotopy)
t1 = t1*gamma_homotopy + dae.addZ('t1_homotopy')*(1 - gamma_homotopy)
t2 = t2*gamma_homotopy + dae.addZ('t2_homotopy')*(1 - gamma_homotopy)
t3 = t3*gamma_homotopy + dae.addZ('t3_homotopy')*(1 - gamma_homotopy)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual( [ode, alg] )
return dae
#
# 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 matplotlib.pyplot as plt
from rawe.dae import Dae
from rawe.collocation import Coll
if __name__ == "__main__":
######## make the Dae #######
dae = Dae()
[pos,vel] = dae.addX( ["pos","vel"] )
dae.addX('abspos')
force = dae.addU( "force" )
# some extra outputs for the dae model
dae['force_over_pos'] = force/pos
# specify the ode residual
mass = 1.0
dae.setResidual([dae.ddt('pos') - vel,
dae.ddt('vel') - (force/mass - 3.0*pos - 0.2*vel)]
)
######## make the collocation scheme ########
N = 100
ocp = Coll(dae, nk=N, nicp=1, collPoly="RADAU", deg=4)
endTime = 5
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX([
'r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z', 'e11', 'e12', 'e13', 'e21',
'e22', 'e23', 'e31', 'e32', 'e33', 'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z', 'aileron', 'elevator', 'rudder',
'flaps', 'prop_drag'
])
dae.addU(['daileron', 'delevator', 'drudder', 'dflaps', 'dprop_drag'])
dae.addP(['r', 'w0'])
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat(
[dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r'] * dae['nu']
dae['R_n2b'] = C.vertcat([
C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])
])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat(
[dae['v_bn_n_x'], dae['v_bn_n_y'], dae['v_bn_n_z']])
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul(dae['R_n2b'], v_bw_n)
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = \
aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
dae['prop_drag_vector'] = dae['prop_drag'] * v_bw_n / dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
# if we are running a homotopy,
# add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([
dae.ddt('r_n2b_n_' + name) - dae['v_bn_n_' + name]
for name in ['x', 'y', 'z']
]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual([ode, alg])
return dae
def crosswindModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "r"
, "dr"
, 'ddr'
, "aileron"
, "elevator"
, "rudder"
, "flaps"
] )
dae.addU( [ "daileron"
, "delevator"
, "drudder"
, "dflaps"
, 'dddr'
] )
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# get mass matrix, rhs
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in \
['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def carouselModel(conf,nSteps=None,extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
# dae.addZ( [ "dddelta"
# , "ddx"
# , "ddy"
# , "ddz"
# , "dw1"
# , "dw2"
# , "dw3"
# , "nu"
# ] )
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w1"
, "w2"
, "w3"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
] )
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta'],
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta']),
dae.ddt('sin_delta') - dae['cos_delta']*dae['ddelta']])
else:
raise ValueErorr('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "motor_torque"
, 'ddr'
] )
# add wind parameter if wind shear is in configuration
if 'z0' in conf and 'zt_roughness' in conf:
dae.addP( ['w0'] )
dae['RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron(deg)'] = dae['aileron']*180/C.pi
dae['elevator(deg)'] = dae['elevator']*180/C.pi
dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
dae['motor power'] = dae['motor_torque']*dae['ddelta']
dae['tether tension'] = dae['r']*dae['nu']
dae['winch power'] = -dae['tether tension']*dae['dr']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# line angle
dae['cos(line angle)'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
if nSteps is not None:
dae.addP('endTime')
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def crosswindModel(conf):
dae = Dae()
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w1"
, "w2"
, "w3"
, "aileron"
, "elevator"
] )
dae.addU( [ "daileron"
, "delevator"
, 'prop_drag'
] )
dae.addP( ['r',
'w0'] )
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['dw1'] = dae.ddt('w1')
dae['dw2'] = dae.ddt('w2')
dae['dw3'] = dae.ddt('w3')
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['tether_tension'] = dae['r']*dae['nu']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
# acceleration
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def crosswindModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX([
"x", "y", "z", "e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32",
"e33", "dx", "dy", "dz", "w_bn_b_x", "w_bn_b_y", "w_bn_b_z", "r", "dr",
'ddr', "aileron", "elevator", "rudder", "flaps"
])
dae.addU(["daileron", "delevator", "drudder", "dflaps", 'dddr'])
dae.addP(['w0'])
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat(
[dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r'] * dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension'] * dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr'] / 0.1 * 60 / (2 * C.pi)
dae['torque'] = dae['tether_tension'] * 0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.vertcat([
C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])
])
# get mass matrix, rhs
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x', 'y', 'z']]) -
C.veccat([dae['dx'], dae['dy'], dae['dz']]),
C.veccat([
dae.ddt(name) for name in
["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"]
]) - dRexp.trans().reshape([9, 1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in \
['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode, alg])
return dae
def crosswindModel(conf,extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
# dae.addZ( [ "ddx"
# , "ddy"
# , "ddz"
# , "dw1"
# , "dw2"
# , "dw3"
# , "nu"
# ] )
dae.addZ("nu")
dae.addX( [ "x" # state 0
, "y" # state 1
, "z" # state 2
, "e11" # state 3
, "e12" # state 4
, "e13" # state 5
, "e21" # state 6
, "e22" # state 7
, "e23" # state 8
, "e31" # state 9
, "e32" # state 10
, "e33" # state 11
, "dx" # state 12
, "dy" # state 13
, "dz" # state 14
, "w1" # state 15
, "w2" # state 16
, "w3" # state 17
, "r" # state 20
, "dr" # state 21
, "aileron"
, "elevator"
] )
dae.addU( [ "daileron"
, "delevator"
, 'ddr'
] )
dae.addP( ['w0'] )
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['dw1'] = dae.ddt('w1')
dae['dw2'] = dae.ddt('w2')
dae['dw3'] = dae.ddt('w3')
dae['aileron(deg)'] = dae['aileron']*180/C.pi
dae['elevator(deg)'] = dae['elevator']*180/C.pi
dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
dae['tether tension'] = dae['r']*dae['nu']
dae['winch power'] = -dae['tether tension']*dae['dr']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
# acceleration
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dae['accel (g)'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel without gravity (g)'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel without gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos(line angle)'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind at altitude']
cL = dae['cL']
cD = dae['cD']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyd's limit"] = loyds
dae["loyd's limit (exact)"] = loyds2
dae['-(winch power)'] = -dae['winch power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae