def main(tf=10., dt=0.005):
np.set_printoptions(linewidth=500)
_fdm = fdm.MR_FDM()
_ctl = ctl.ZAttController(_fdm)
sim = pmu.Sim(_fdm, _ctl)
time = np.arange(0, tf, dt)
X, U = np.zeros((len(time), fdm.sv_size)), np.zeros((len(time), fdm.iv_size))
Yc = np.zeros((len(time), ctl.iv_size))
#_in = ctl.CstInput(0, [np.deg2rad(0.1), 0, 0])
#_in = ctl.SinZInput()
_in = ctl.RandomInput()
#_in = ctl.StepEulerInput(pal.e_phi, _a=np.deg2rad(1.), p=4, dt=1)
#_in = ctl.StepEulerInput(pal.e_theta)
#_in = ctl.StepEulerInput(pal.e_psi)
X[0] = sim.reset(time[0])
for i in range(1, len(time)):
Yc[i-1] = _in.get(time[i-1])
sim.Yc = Yc[i-1]
U[i-1], X[i] = sim.run(time[i])
Yc[-1] = _in.get(time[-1])
U[-1], unused = sim.run(time[-1])
sim.fdm.plot(time, X, U)
sim.ctl.plot(time, Yc, U)
plt.show()
def main():
logging.basicConfig(level=logging.INFO)
np.set_printoptions(linewidth=500)
param_filename = '/home/poine/work/pat/data/vehicles/cularis.xml'
trim_args = {'h': 0, 'va': 10, 'gamma': 0, 'flaps': 0}
_fdm = p1_fw_dyn.DynamicModel(param_filename)
_fms = p3_guid.FMS(_fdm, trim_args)
_atm = p3_atm.AtmosphereCalm()
#_atm = p3_atm.AtmosphereThermalMulti()
sim = p3_u.Sim(_fdm, _fms, _atm)
t0, tf, dt = 0, 5., 0.01
time = np.arange(t0, tf, dt)
Xe, Ue = _fdm.trim(trim_args, report=True)
sim.reset(time[0], Xe, Ue)
sim.ctl.set_mode(p3_guid.FMS.mod_auto1)
for i in range(1, len(time)):
sim.run(time[i])
_fdm.plot_trajectory(time,
np.array(sim.Xs),
np.array(sim.Us),
window_title="None")
plt.show()
def main(dt=0.005):
args = parse_command_line()
if args.list:
print('available trajectories:')
for n in pmtf.list():
print(' {}'.format(n))
try:
print('loading trajectory: {}'.format(args.traj))
_traj, _desc = pmtf.get(args.traj)
print(' desc: {}'.format(_desc))
except KeyError:
print('unknown trajectory {}'.format(args.traj))
return
_fdm = fdm.MR_FDM()
#_fdm = fdm.UFOFDM()
#_fdm = fdm.SolidFDM()
_ctl_input = ctl.TrajRef(_traj, _fdm)
_ctl = ctl.PosController(_fdm, _ctl_input)
sim = pmu.Sim(_fdm, _ctl)
time = np.arange(0, _traj.duration, dt)
X, U = np.zeros((len(time), fdm.sv_size)), np.zeros(
(len(time), _fdm.iv_size))
Xref = np.zeros((len(time), _ctl.ref_size))
X[0] = sim.reset(time[0], _ctl_input.get(time[0])[2])
for i in range(1, len(time)):
U[i - 1], X[i] = sim.run(time[i])
Xref[i - 1] = _ctl.Xref
U[-1], Xref[-1] = U[-2], Xref[-2]
figure = _fdm.plot(time, X, U)
_ctl.plot(time, U, Xref)
plt.show()
def __init__(self, _traj=None, time_factor=1.):
self.time_factor = time_factor
_fdm = fdm.MR_FDM()
#_fdm = fdm.UFOFDM()
#_fdm = fdm.SolidFDM()
self.traj_pub = pru.TrajectoryPublisher(_traj)
_ctl_in = ctl.TrajRef(_traj, _fdm)
_ctl = ctl.PosController(_fdm, _ctl_in)
self.sim = pmu.Sim(_fdm, _ctl)
self.tf_pub = pru.TransformPublisher()
self.pose_pub = pru.QuadAndRefPublisher()
def __init__(self):
_fdm = fdm.FDM()
_ctl = ctl.ZAttController(_fdm)
self.sim = pmu.Sim(_fdm, _ctl)
self.tf_pub = pru.TransformPublisher()
self.pose_pub = pru.PoseArrayPublisher()
rospy.Subscriber('/joy',
sensor_msgs.msg.Joy,
self.on_joy_msg,
queue_size=1)
#self.input = ctl.StepZInput(0.1)
#self.input = ctl.StepEulerInput(pal.e_phi, _a=np.deg2rad(1.), p=4, dt=1)
self.input = ctl.StepEulerInput(pal.e_theta,
_a=np.deg2rad(1.),
p=4,
dt=1)
def __init__(self):
_fdm = fdm.FDM()
if 0:
_ctl_in = ctl.PosStep()
self.traj_pub = None
else:
#_traj = pmt.Circle([0, 0, -0.5], r=1.5, v=2.)
_traj = pmt.FigureOfEight(v=1.)
#_traj = pmt.Oval(l=1, r=1, v=2.)
self.traj_pub = pru.TrajectoryPublisher(_traj)
_ctl_in = ctl.TrajRef(_traj, _fdm)
_ctl = ctl.PosController(_fdm, _ctl_in)
self.sim = pmu.Sim(_fdm, _ctl)
self.tf_pub = pru.TransformPublisher()
self.pose_pub = pru.QuadAndRefPublisher()
def __init__(self, _traj=None, time_factor=1.):
traj = p3_traj3d.CircleRefTraj(c=[0, 0, -20], r=15)
#traj = p3_fw_guid.LineRefTraj(p1=[0, 0, 0], p2=[50, 50, 0])
#traj = p3_fw_guid.SquareRefTraj()
#traj = p3_fw_guid.OctogonRefTraj()
self.time_factor = time_factor
param_filename = '/home/poine/work/pat/data/vehicles/cularis.xml'
_fdm = p1_fw_dyn.DynamicModel(param_filename)
_fms = p3_fms.FMS(_fdm, {'h': 0, 'va': 10, 'gamma': 0})
#_ctl = p3_fw_guid.Guidance(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_ctl = p3_guid.GuidanceThermal(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_atm = p3_atm.AtmosphereCstWind([0, 0, -1])
#_atm = p3_atm.AtmosphereThermal1()
_atm = p3_atm.AtmosphereThermalMulti()
#_atm = p3_atm.AtmosphereWharington()#center=None, radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereGedeon(center=[55, 0, 0], radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereRidge()
self.sim = pmu.Sim(_fdm, _fms, _atm)
self.tf_pub = p3_rpu.TransformPublisher()
self.carrot_pub = p3_rpu.MarkerPublisher('guidance/goal',
'w_ned',
scale=(0.25, 0.25, 0.5))
self.traj_pub = p3_rpu.TrajectoryPublisher2(traj, ms=0.2)
#self.pose_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
self.pose_pub = p3_rpu.PoseArrayPublisher(dae='ds_glider_full.dae')
self.atm_pub = None #p3_rpu.AtmPointCloudPublisher(_atm)
self.status_pub = p3_rpu.GuidanceStatusPublisher()
self.atm_disp_dyn_rec_client = dynamic_reconfigure.client.Client(
"display_atmosphere",
timeout=1,
config_callback=self.atm_config_callback)
#self.my_own_reconfigure_client = dynamic_reconfigure.client.Client("real_time_sim", timeout=1, config_callback=None)
self.cur_thermal_id = 0
self.dyn_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.guidanceConfig,
self.dyn_cfg_callback)
# we can not have two servers... f**k...
#self.atm_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.atmosphereConfig,
# self.atm_dyn_cfg_callback)#, subname='foo')
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
geometry_msgs.msg.PoseStamped,
self.nav_goal_callback)
self.joy_sub = rospy.Subscriber('/joy', sensor_msgs.msg.Joy,
self.joy_callback)
self.periodic_cnt = 0
def main(traj, dt=0.005):
_fdm = fdm.FDM()
_ctl_input = ctl.TrajRef(traj, _fdm)
_ctl = ctl.PosController(_fdm, _ctl_input)
sim = pmu.Sim(_fdm, _ctl)
time = np.arange(0, traj.duration, dt)
X, U = np.zeros((len(time), fdm.sv_size)), np.zeros(
(len(time), fdm.iv_size))
Xref = np.zeros((len(time), _ctl.ref_size))
X[0] = sim.reset(time[0], _ctl_input.get(time[0])[2])
for i in range(1, len(time)):
U[i - 1], X[i] = sim.run(time[i])
Xref[i - 1] = _ctl.Xref
U[-1], Xref[-1] = U[-2], Xref[-2]
figure = _fdm.plot(time, X, U)
_ctl.plot(time, U, Xref)
plt.show()