def main():
rospy.init_node('test_trajectory')
traj_name = rospy.get_param('~traj_name', 'circle4')
if rospy.get_param('~list_available', False):
rospy.loginfo(' available trajectories\n{}'.format(pmtf.list()))
try:
rospy.loginfo(' Loading trajectory: {}'.format(traj_name))
traj, desc = pmtf.get(traj_name)
rospy.loginfo(' Description: {}'.format(desc))
Agent(traj, time_factor=0.5).run()
except KeyError:
rospy.loginfo(' Unkwown trajectory: {}'.format(traj_name))
rospy.loginfo(' available trajectories\n{}'.format(pmtf.list()))
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 main():
args = parse_command_line()
if args.list_traj:
print('available trajectories:')
for n in pmtf.list():
print(' {}'.format(n))
return
traj, desc = pmtf.get(args.traj)
print('loading trajectory {} ({})'.format(args.traj, desc))
t0, t1, dt = 0., args.repeat * traj.duration, 0.01
time = np.arange(t0, t1, dt)
Yc = np.array([traj.get(t) for t in time])
if 1:
_fdm = fdm.FDM()
df = ctl.DiffFlatness()
Xc, Uc = [], []
for Yci in Yc:
Xci, Uci = df.state_and_cmd_of_flat_output(Yci, _fdm.P)
Xc.append(Xci)
Uc.append(Uci)
Xc = np.array(Xc)
Uc = np.array(Uc)
_fdm.plot(time, Xc, Uc)
#pdb.set_trace()
pmt.plot(time, Yc)
pmt.plot3d(time, Yc)
plt.show()