def main(): rospy.init_node('real_time_sim') traj_name = rospy.get_param('~traj_name', 'circle_with_intro_slow') rospy.loginfo(' Loading trajectory: {}'.format(traj_name)) traj, desc = pmtf.get(traj_name) time_factor = rospy.get_param('~time_factor', 1.) Agent(traj, time_factor).run()
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()
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(): args = parse_command_line() if args.list: pmtf.print_available() return try: print('loading trajectory: {}'.format(args.traj)) traj, desc = pmtf.get(args.traj) print(' description: {}'.format(desc)) except KeyError: print('unknown trajectory {}'.format(args.traj)) return 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: plot_dflat_state_and_input(time, Yc) pmt.plot(time, Yc) pmt.plot3d(time, Yc) plt.show()
def main(): rospy.init_node('test_trajectory') traj_name = 'circle4' traj, desc = pmtf.get(traj_name) Agent(traj, time_factor=0.5).run()