def main(tf=10., dt=0.01):
np.set_printoptions(linewidth=500)
P = fdm.Param()
_fdm = fdm.FDM()
_ctl = ctl.ZAttController(_fdm)
time = np.arange(0, tf, dt)
#Yc = step_phi(time)
Yc = step_euler(time, pal.e_phi, 1.)
#Yc = step_euler(time, pal.e_theta, 1.)
#Yc = step_euler2(time, pal.e_psi, np.deg2rad(1.))
#Yc = step_z(time)
Xe, Ue = fdm.trim(P)
X0 = np.array(Xe)
#X0[fdm.sv_zd] += 0.1
#X0[fdm.sv_slice_quat] = pal.quat_of_euler([np.deg2rad(1.), 0., 0.])
X, U = np.zeros((len(time), fdm.sv_size)), Ue*np.ones((len(time), fdm.iv_size))
X[0] = _fdm.reset(X0, time[0])
for i in range(1, len(time)):
U[i-1] = _ctl.get(X[i-1], Yc[i-1])
X[i] = _fdm.run(time[i], U[i-1])
fdm.plot(time, X, U)
plot_sp(time, Yc)
plt.show()
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 __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)