def test1():
param_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/skywalker_x8.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
plot_polar(dm)
#trim_throttle(dm)
plot_trims(dm)
plt.show()
def __init__(self):
self._st = sym.symbols('t')
self._sx, self._sy, self._sz, self._sxd, self._syd, self._szd , self._spsi = sym.symbols('x, y, z, xd, yd, zd, psi', cls=sym.Function)
self._sphi, self._stheta, self._sthrust = sym.symbols('phi, theta, thrust', cls=sym.Function)
self._state_symbols = (self._sx(self._st), self._sy(self._st), self._sz(self._st),
self._sxd(self._st), self._syd(self._st), self._szd(self._st), self._spsi(self._st))
#self._input_symbols = (self._sv, self._sphi)
self.P = p3_fw_dynp.Param(os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml'))
def __init__(self, params=None):
if params is None:
params = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
self.P = Param(params)
self.X = np.zeros(self.get_sv_size()) # State Vector
self.X_act = np.zeros(self.input_nb()) # Actuators State Vector
self.T_w2b = np.eye(4) # 4x4 homogenous transform from world to body
self.t, self.dt = 0., 0.01
self.reset()
def main():
param_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
pc = ScheduledPitchControl(dm)
run_sim(dm, pc)
#old_thing(dm)
plt.show()
def get_trim_defaults(trim_args={
'h': 0,
'va': 12,
'gamma': 0
},
ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True)
return dm, Xe, Ue
def plot_poles(trim_args={'h': 0, 'va': 12, 'gamma': 0}, ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True)
A, B = dm.get_jacobian(Xe, Ue)
_eval, _evel = np.linalg.eig(A)
B1 = B[:, 2][np.newaxis].T
C = np.array([0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0])
D = np.zeros((1, 1))
ss = control.ss(A, B1, C, D)
tf = control.ss2tf(ss)
pdb.set_trace()
def __init__(self, name='ros_pat_glider_trim'):
p3_rpu.PeriodicNode.__init__(self, name)
self.joint_state_pub = p3_rpu.JointStatePublisher(what=name)
#self.ivel_pub = p3_rpu.PosePublisher(what=name)
self.txt_pub = p3_rpu.TextMarkerPublisher('/ds_glider/trim_txt', 'ds_glider/base_link', scale=0.1, what=name)
self.tf_pub = p3_rpu.TransformPublisher()
self.tf_pub.send_w_enu_to_ned_transform(rospy.Time.now())
ac_name = 'cularis'
self.dm = p1_fw_dyn.DynamicModel(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
self.trim_args = {'h':0, 'va':10, 'throttle':0}
self._trim()
self.dyn_cfg_srv = dynamic_reconfigure.server.Server(ros_pat.cfg.glider_trimConfig, self.dyn_cfg_callback)
def main():
param_filename = os.path.join(p3_u.pat_dir(),
'data/vehicles/skywalker_x8.xml')
P = pat3.vehicles.fixed_wing.simple_6dof_fdm_param.Param(param_filename)
XFLR5_to_AVL(P)
print(P)
test_alpha(P)
alpha, beta, rvel = 0., 0., [0., 0., 0.]
Usfc = [0, 0, 0, 0]
(CL1, CY1, CD1), (CL2, CY2, CD2) = get_both_coefs(P, alpha, beta, rvel,
Usfc)
print(CL1, CY1, CD1)
print(CL2, CY2, CD2)
def test01(trim_args={
'h': 0,
'va': 15,
'throttle': 0,
'flaps': np.deg2rad(-8.)
},
ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
#dm = p1_fw_dyn.DynamicModel_ee(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
#dm = p1_fw_dyn.DynamicModel_eq(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True, report=True, aero=False)
print Xe, Ue
Xe, Ue = dm.trim(trim_args, debug=True, report=True, aero=True)
print Xe, Ue
return dm, Xe, Ue
def test0():
h = 0
aero_vel = va, alpha, beta = [10, 0, 0]
euler, rvel = [0., 0, 0], [0.1, 0, 0]
Usfc = [0, 0, 0, 0]
Ueng = [0.1]
p_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
P = p3_fwparam.Param(p_filename)
rho = p3_atm.get_rho(h)
Pdyn = 0.5*rho*va**2
X = np.concatenate(([0, 0, -h], aero_vel, euler, rvel))
print('f_eng new: {}'.format(p3_fw_aero.get_f_eng_body(h, va, Ueng, P)))
print('f_eng old: {}'.format(p1_fw_dyn.get_f_eng_body(X, Ueng, P)))
print('new: {}'.format(p1_fw_dyn.get_m_aero_body2(va, alpha, beta, rvel, Usfc, P, Pdyn)))
print('old {}'.format(p1_fw_dyn.get_m_aero_body(X, Usfc, P, Pdyn)))
def get_default_dm(ac_name=DEFAULT_AC):
#return p1_fw_dyn.DynamicModel(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
return p1_fw_dyn.DynamicModel_ee(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
ax = plt.subplot(2, 3, 2)
wc1 = [
-atm.get_wind([_x, _y, -h], t=0)[2]
for _x, _y in zip(c_pts[:, 0], c_pts[:, 1])
]
plt.plot(np.rad2deg(alphas), wc1)
ax = plt.subplot(2, 3, 5)
plt.plot(time, wc1)
ax = plt.subplot(2, 3, 6)
_foo(alphas, wc1, dt, time, _c)
def test_circles(atm, xmax=50, dx=5.):
pass
def main(param_filename):
dm = p1_fw_dyn.DynamicModel(param_filename)
atm = p3_atm.AtmosphereThermal1()
#atm.set_params(xc=0, yc=10, zi=1500, wstar=300)
#find_best_radius(dm, atm, va=9., compute=True, plot_atm=False, plot_traj=True)
plot_gradient(atm)
plt.show()
if __name__ == "__main__":
logging.basicConfig(level=logging.INFO)
np.set_printoptions(linewidth=500)
param_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
main(param_filename)
def get_default_dm(ac_name='cularis'): #ac_name='skywalker_x8'):
return p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))