def main(*, config=None):
import logcolor
logcolor.basic_config(level=logging.INFO)
if config is None:
config = default_config_cls()
log.info('loading from config %r', config)
# This trickery runs DAgger in a separate thread, and makes sure the thread
# dies with the main thread. This is simply because the Qt GUI library must
# run in the main thread on at least macOS, due to Apple engineering - and
# who knows, maybe they have good reasons.
if config.show_tracker:
from squad.traj import Tracker
from threading import Thread
tracker = Tracker(plot_opts=dict(color='ff0000'),
autopan=False,
elevation=0,
azimuth=-90,
distance=100.0)
d = Dagger(config=config, tracker=tracker)
t = Thread(target=d.run, daemon=True)
t.start()
try:
with forward_exceptions(t):
tracker.show()
if t.is_alive():
raise KeyboardInterrupt
except KeyboardInterrupt:
log.warning('keyboard interrupt, quitting')
else:
d = Dagger(config=config)
d.run()
def main(args=sys.argv[1:]):
rospy.init_node('aumav_plan')
logcolor.basic_config(level=logging.INFO)
class Robot(RCMixin, BaseRobot): pass
r = Robot()
r.run(r.wait_for_init())
log.info('i am robot')
#r.run(r.hover_and_land(r.move_planned((1,2,3))))
#r.run(r.hover_and_land(r.scan()))
#r.run(r.hover_and_land(r.publish_setpoint((0, 0, 1.0))))
r.run(r.hover_and_land(r.rc_control()))
def __init__(self,
position=(0.0, 0.0),
rotation=0.0,
home=None,
grid=None):
logcolor.basic_config(level=logging.DEBUG)
self.position = np.r_[position]
self.home_position = np.r_[position if home is None else home]
self.rotation = rotation
self.current_path = None
self.grid = grid
self.update_grid()
with open(path.expanduser('~/mrbrain-things.txt')) as f:
self.things = [Thing(*eval(line)) for line in f]
def main(args=sys.argv[1:]):
rospy.init_node('covermapper')
logcolor.basic_config(level=logging.INFO)
args = rospy.myargv(args)
if not use_jit:
log.warn('numba installed or failed to import. '
'mapping will be vely slow!')
dfl_map_fn = path.expanduser('~/map.txt')
mb = CoverageMapBuilder((args[:1] + [dfl_map_fn])[0])
rate = rospy.Rate(4)
while not rospy.is_shutdown():
mb.publish_map()
rate.sleep()
def main(args=sys.argv[1:]):
logcolor.basic_config(level=logging.INFO)
model_spec, policy_spec, *init_state = args
tracker = traj.Tracker()
model = import_name(model_spec)()
policy = import_name(policy_spec)()
num_samples = env_param('num_samples', default=1, cast=int)
try:
policy.tracker = tracker.alt(color='g')
except Exception as e:
log.warn('could not set tracker attribute: %s', e)
if init_state:
vals = list(map(eval, init_state))
x0s = np.array(vals, dtype=np.float64)
x0s = x0s.reshape((num_samples, *model.state_shape))
else:
x0s = model.sample_states(num_samples)
log.info('initial states:\n%s', model.state_rep(x0s))
dt = env_param('dt', default=1 / 400, cast=float)
policy_noise = lambda x: np.random.normal(policy(x), 1e-2)
thread_unroll = threadwrap(model.unroll)
ts = [
thread_unroll(policy=policy_noise,
x0=x0,
dt=dt,
callback=tracker.alt().set_history,
t_min=env_param('t_min', default=1.0, cast=float),
t_max=env_param('t_max', default=60.0, cast=float),
q_min=env_param('q_min', default=1e-4, cast=float))
for x0 in x0s
]
tracker.autopan = (len(ts) == 1)
tracker.show()
with forward_exceptions(*ts):
for t in ts:
x, history = t.join()
log.info('final state:\n%s', model.state_rep(x))
trj = traj.Trajectory.from_history(history)
trj.save('.last.npz')
def main(dt=DT, T=Tl, Tl=Tl, Tstep=Tl, n=10):
import logcolor
logcolor.basic_config(level=logging.DEBUG)
solver = make_quad(dt=dt, T=T)
model = solver.model
initials = []
optimizeds = []
for i, x0 in enumerate(model.sample_states(n)):
log.info('sample %d, x0:\n%s', i, model.state_rep(x0))
states, actions = solver.initial_traj(x0, T=T)
initials.append(
traj.Trajectory(dt=dt,
states=states.copy(),
actions=actions.copy()))
initial = solver.initial_traj(x0, T=Tl).actions
for j in range(0, T, Tstep):
k = j + Tstep
states_j, actions_j = solver(states[j],
initial=initial,
atol=5e0,
λ_base=3.0,
ln_λ=-5,
ln_λ_max=20,
iter_max=2000)
states[j:k + 1], actions[j:k] = states_j[:Tstep +
1], actions_j[:Tstep]
#filler = np.zeros((Tstep, *model.action_shape))
#filler = solver.initial_traj(states[k], T=Tstep).actions
filler = np.repeat(actions_j[-1][None], Tstep, axis=0)
#filler = np.random.uniform(u_lower, u_upper, size=(Tstep, *model.action_shape))
initial = np.vstack((actions_j[Tstep:], filler))
#states = model.step_array(x0, actions, dt=dt)
log.info('x_final:\n%s', model.state_rep(states[-1]))
optimizeds.append(
traj.Trajectory(dt=dt, states=states, actions=actions))
traj.save_many('pid_trajs.npz', initials)
traj.save_many('ddp_trajs.npz', optimizeds)
def main(args=sys.argv[1:]):
logcolor.basic_config()
histories = []
while args:
arg = args.pop(0)
if arg == 'load':
fn = args.pop(0)
history = load(fn)
histories.append(history)
log.info("loaded '%s' (%d steps)", fn, len(history))
elif arg == 'loadmany':
fn = args.pop(0)
histories_fn = list(load_many(fn))
histories.extend(histories_fn)
log.info("loaded '%s' (%d trajectories)", fn, len(histories_fn))
for i, history in enumerate(histories_fn):
log.info("- trajectory %d (%d steps)", i, len(history))
elif arg == 'select':
num = int(args.pop(0))
history = histories[num]
elif arg == 'savemany':
fn = args.pop(0)
save_many(fn, histories)
log.info("saving to '%s' (%d trajectories)", fn, len(histories))
elif arg == 'animate' or arg == 'video':
plots.animation_frames(*histories)
elif arg == 'plot' or arg == 'evaluate':
plots.evaluation(history)
elif arg == 'showtraj':
plots.trajectory(*histories)
elif arg == 'viewinit':
viewer = Viewer()
elif arg == 'viewtraj':
for history in histories:
viewer.plot_traj(history)
elif arg == 'viewtraj2':
viewer.plot_traj(history, color='g', width=0.2)
elif arg == 'viewpan':
viewer.pan_start(history)
elif arg == 'viewshow':
viewer.show()
else:
log.error('unknown command: %s', arg)
def main(args=sys.argv[1:]):
logcolor.basic_config(level=logging.DEBUG)
if ''.join(args[:1]) == 'cli':
cli()
else:
gui()