def plot_ts():
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml'
)
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load transition system
ts_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/transition_system.txt'
with open(ts_filename, 'r') as fin:
data = yaml.load(fin)
ts = Ts(multi=False)
ts.g.add_edges_from(data['ts'])
ts.init[data['ts_init']] = 1
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0])
plt.ylim(b.T[1])
mission.draw_environment(axes, figure, origin=np.array((0, 0)))
draw_ts(ts, axes, figure)
path = None
with open(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/policy_new.txt',
'r') as fin:
lines = fin.readlines()
lines = ''.join(map(str.strip, lines))
exec lines
print path
axes.plot([p[0] for p in path], [p[1] for p in path],
'o',
color=(1, 0.5, 0),
markersize=10)
for u, v in it.izip(path[:-1], path[1:]):
x, y = np.array([u[:2], v[:2]]).T
axes.plot(x, y, color=(1, 0.5, 0), linestyle='-', lw=4)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
for img in ['png', 'jpg', 'pdf', 'svg']:
plt.savefig(
'/home/cristi/Dropbox/FIRM/Experiments/bu_case/bu_ts.{}'.format(
img))
plt.show()
def plan():
# 0. parse command-line arguments
parser = argparse.ArgumentParser(
description='''Sampling-based mission planner with Distribution Temporal
Logic specifications.''',
epilog=license_text)
parser.add_argument('mission_file', type=str, help='mission file')
parser.add_argument('-o', '--output-dir', type=str, help='output directory')
parser.add_argument('-l', '--logfile', type=str, help='log file',
default='gdtlfirm.log')
parser.add_argument('-d', '--design', action='store_true',
help='only show environment')
parser.add_argument('-p', '--plot', choices=['figures', 'video', 'none'],
help='plot simulation data', default='none')
parser.add_argument('--debug', action='store_true',
help='debug mode')
parser.add_argument('-v', '--verbose', action='store_true',
help='print/plot extra information for simulation')
# parse arguments
args = parser.parse_args()
# create output directory
if not os.path.isdir(args.output_dir):
os.makedirs(args.output_dir)
# configure logging
fs, dfs = '%(asctime)s %(levelname)s %(message)s', '%m/%d/%Y %I:%M:%S %p'
loglevel = logging.INFO #logging.DEBUG if args.debug else logging.INFO
logging.basicConfig(filename=args.logfile, level=loglevel, format=fs, datefmt=dfs)
if args.verbose:
root = logging.getLogger()
ch = logging.StreamHandler(sys.stdout)
ch.setLevel(loglevel)
ch.setFormatter(logging.Formatter(fs, dfs))
root.addHandler(ch)
# 1. load mission and environment
mission = Mission.from_file(args.mission_file)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.planning.get('seed', None))
if args.design: # only shows the environment
mission.visualize(None, plot='design')
return
# 2. setup, find solution and execute it
my_setup = FIRM2DSetup()
# setup planner
my_setup.setup(mission)
# solve problem and if successful execute it
fname = os.path.join(args.output_dir,
mission.planning.get('solution_filename', None))
if my_setup.solve(save=fname):
my_setup.execute_solution()
logging.info('Plan Executed Successfully.')
else:
logging.info('Unable to find Solution in given time.')
logging.info('Task Complete')
mission.visualize(my_setup.planner.ts, plot='plot')
def extract_ts():
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml'
)
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load transition system
ts_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/transition_system.txt'
with open(ts_filename, 'r') as fin:
data = yaml.load(fin)
ts = Ts(multi=False)
ts.g.add_edges_from(data['ts'])
ts.init[data['ts_init']] = 1
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0])
plt.ylim(b.T[1])
mission.draw_environment(axes, figure, origin=np.array((0, 0)))
draw_ts(ts, axes, figure)
policy_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/policy_new.txt'
with open(policy_filename, 'w') as fout:
print >> fout, ' path = np.array(['
def onclick(event):
print('button=%d, x=%d, y=%d, xdata=%f, ydata=%f' %
(event.button, event.x, event.y, event.xdata, event.ydata))
node, d = min([(p, dist(p[:2], (event.xdata, event.ydata)))
for p in ts.g.nodes_iter()],
key=lambda x: x[1])
print node, d
assert ts.g.has_node(node)
with open(policy_filename, 'a') as fout:
print >> fout, ' {},'.format(node)
cid = figure.canvas.mpl_connect('button_press_event', onclick)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
plt.show()
with open(policy_filename, 'a') as fout:
print >> fout, ' ]*10)'
def executeController(args, controls, desired_trajectory):
# create robot object
if args.robot == 'X80Pro1':
robot = X80Pro('DrRobot1', '192.168.0.202', 10001)
elif args.robot == 'X80Pro2':
robot = X80Pro('DrRobot1', '192.168.0.201', 10001)
elif args.robot == 'ScoutPro1':
robot = ScoutPro('DrRobot1', '192.168.0.220', 10002)
elif args.robot == 'ScoutPro2':
robot = ScoutPro('DrRobot1', '192.168.0.218', 10002)
else:
raise ValueError('Unknown robot!')
real_trajectory = []
try:
for k, u in enumerate(controls):
# for k, u in enumerate(it.repeat([0.2, 0], times=4*3)):
z = getTruePose(args.id)
logging.info('Step %d: true pose: %s', k, z)
real_trajectory.append(z)
executeControl(robot, u)
finally:
robot.setSpeed([0, 0])
# 5. plot
if args.plot:
alen = 0.05
# 1. load mission and environment
mission = Mission.from_file(args.mission_file)
region_data = mission.environment['regions']
regions = dict([(reg.lower(), (np.array(d['position']), np.array(d['sides'])))
for reg, d in region_data.iteritems()])
regions.update([(reg, np.array([[c[0] - s[0]/2.0, c[0] + s[0]/2.0],
[c[1] - s[1]/2.0, c[1] + s[1]/2.0]]))
for reg, (c, s) in regions.iteritems()])
region_colors = dict([(reg.lower(), d['color'])
for reg, d in region_data.iteritems()])
figure = plt.figure()
plt.axis('equal')
for reg, b in regions.iteritems():
x, y = b[0, 0], b[1, 0]
l, w = b[0, 1]-x, b[1, 1]-y
r = plt.Rectangle((x, y), l, w, color=region_colors[reg], fill=True,
linewidth=2)
figure.gca().add_artist(r)
plt.text(np.sum(b[0])/2, np.sum(b[1])/2, reg)
for x, y, yaw in desired_trajectory:
plt.arrow(x, y, alen*np.cos(yaw), alen*np.sin(yaw), hold=True, color='green')
for x, y, yaw in real_trajectory:
plt.arrow(x, y, 0.1*np.cos(yaw), 0.1*np.sin(yaw), hold=True, color='k')
xi, yi, yawi = desired_trajectory[0]
plt.arrow(xi, yi, alen*np.cos(yawi), alen*np.sin(yawi), hold=True, color='blue')
plt.xlim([0, 4.13])
plt.ylim([0, 3.54])
plt.show()
def extract_ts():
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml')
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load transition system
ts_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/transition_system.txt'
with open(ts_filename, 'r') as fin:
data = yaml.load(fin)
ts = Ts(multi=False)
ts.g.add_edges_from(data['ts'])
ts.init[data['ts_init']] = 1
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0])
plt.ylim(b.T[1])
mission.draw_environment(axes, figure, origin=np.array((0, 0)))
draw_ts(ts, axes, figure)
policy_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/policy_new.txt'
with open(policy_filename, 'w') as fout:
print>>fout, ' path = np.array(['
def onclick(event):
print('button=%d, x=%d, y=%d, xdata=%f, ydata=%f' %
(event.button, event.x, event.y, event.xdata, event.ydata))
node, d = min([(p, dist(p[:2], (event.xdata, event.ydata)))
for p in ts.g.nodes_iter()],
key=lambda x: x[1])
print node, d
assert ts.g.has_node(node)
with open(policy_filename, 'a') as fout:
print>>fout, ' {},'.format(node)
cid = figure.canvas.mpl_connect('button_press_event', onclick)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
plt.show()
with open(policy_filename, 'a') as fout:
print>>fout, ' ]*10)'
def plot_ts():
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml')
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load transition system
ts_filename = '/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/transition_system.txt'
with open(ts_filename, 'r') as fin:
data = yaml.load(fin)
ts = Ts(multi=False)
ts.g.add_edges_from(data['ts'])
ts.init[data['ts_init']] = 1
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0])
plt.ylim(b.T[1])
mission.draw_environment(axes, figure, origin=np.array((0, 0)))
draw_ts(ts, axes, figure)
path = None
with open('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/policy_new.txt', 'r') as fin:
lines = fin.readlines()
lines = ''.join(map(str.strip, lines))
exec lines
print path
axes.plot([p[0] for p in path], [p[1] for p in path], 'o', color=(1, 0.5, 0), markersize=10)
for u, v in it.izip(path[:-1], path[1:]):
x, y = np.array([u[:2], v[:2]]).T
axes.plot(x, y, color=(1, 0.5, 0), linestyle='-', lw=4)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
for img in ['png', 'jpg', 'pdf', 'svg']:
plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/bu_ts.{}'.format(img))
plt.show()
def plot_experiments_cov():
# 1. load mission and environment
# mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml'
)
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# origin = np.array([-0.08, 0.696, 0])
origin = np.array([0, 0, 0])
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0] - origin[0])
plt.ylim(b.T[1] - origin[1])
mission.draw_environment(axes, figure, origin=origin[:2])
# for experiment in range(1002, 1012):
# draw_experiment_data(experiment, axes, estimate=False, camera=False, truth=False, cov=True)
experiment = 1010
draw_experiment_data(experiment,
axes,
estimate=True,
camera=True,
truth=True,
cov=True)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
for img in ['png', 'jpg', 'pdf', 'svg']:
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_estimate_and_cov_{}.{}'.format(experiment, img))
plt.savefig(
'/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_all_pose_and_cov_{}.{}'
.format(experiment, img))
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_cov_{}.{}'.format(experiment, img))
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/all_experiments_ground_truth_only.{}'.format(img))
plt.show()
def plot_experiments_cov():
# 1. load mission and environment
# mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/bu/mission.yaml')
# logging.info('\n' + str(mission))
# logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# origin = np.array([-0.08, 0.696, 0])
origin = np.array([0, 0, 0])
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
b = np.array(env['boundary']['limits'])
plt.xlim(b.T[0] - origin[0])
plt.ylim(b.T[1] - origin[1])
mission.draw_environment(axes, figure, origin=origin[:2])
# for experiment in range(1002, 1012):
# draw_experiment_data(experiment, axes, estimate=False, camera=False, truth=False, cov=True)
experiment = 1010
draw_experiment_data(experiment, axes, estimate=True, camera=True, truth=True, cov=True)
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.tight_layout(pad=0.5)
for img in ['png', 'jpg', 'pdf', 'svg']:
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_estimate_and_cov_{}.{}'.format(experiment, img))
plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_all_pose_and_cov_{}.{}'.format(experiment, img))
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/experiment_cov_{}.{}'.format(experiment, img))
# plt.savefig('/home/cristi/Dropbox/FIRM/Experiments/bu_case/all_experiments_ground_truth_only.{}'.format(img))
plt.show()
def test_observation_model():
isSimulation = True
if not isSimulation:
rospy.init_node("ObservationModelTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
if not isSimulation:
rospy.spin()
def test_observation_model():
isSimulation = True
if not isSimulation:
rospy.init_node("ObservationModelTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
if not isSimulation:
rospy.spin()
def test_unicycle_model():
isSimulation = True
if not isSimulation:
rospy.init_node("MotionModelTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
robot = UnicycleMotionModel(robotModel)
print 'Time step:', robot.dt
state = SE2BeliefState(1, 2, np.pi/4)
control = np.array([200, 100])
print 'State Jacobian:'
print robot.getStateJacobian(state, control)
print 'Control Jacobian:'
print robot.getControlJacobian(state, control)
print 'Noise Jacobian:'
print robot.getNoiseJacobian(state, control)
print 'Noise Covariance Matrix'
print robot.processNoiseCovariance(state, control)
start = SE2BeliefState(0, 0, 0)
goal = SE2BeliefState(1, 1, np.pi/2)
print 'Open loop path:'
policy = robot.generateOpenLoopControls(start, goal)
aux_state = start.copy()
trajectory = [start]
print 'Start:', aux_state.getXYYaw()
for u in policy:
aux_state = robot.evolve(aux_state, u)
print 'Control:', u, 'State:', aux_state.getXYYaw()
trajectory.append(aux_state)
print len(policy)
print 'Generate sample path from open loop policy'
aux_state = start.copy()
sample_path = [aux_state] # without initial uncertainty
for u in policy:
w = robot.generateNoise(aux_state, u)
aux_state = robot.evolve(aux_state, u, w)
print 'Control:', u, 'Noise:', w, 'State:', aux_state
sample_path.append(aux_state)
plt.figure()
for s, sn in zip(trajectory, sample_path):
plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='k')
plt.arrow(sn.x, sn.y, 0.1*np.cos(sn.yaw), 0.1*np.sin(sn.yaw), hold=True, color='b')
plt.xlim([-0.3, 1.3])
plt.ylim([-0.3, 1.3])
plt.show()
print
if not isSimulation:
rospy.spin()
def test_controller():
# from optitrack import OptiTrackInterface
from numpy.linalg import norm
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
cc = mission.planning['setup']['controller']
SE2BeliefState.covNormWeight = 0 * cc['norm_covariance_weight']
SE2BeliefState.meanNormWeight = 1 #cc['norm_mean_weight']
SE2BeliefState.reachDist = cc['reach_dist']
# set the state component norm weights
SE2BeliefState.normWeights = np.array([1, 1,
0]) #cc['norm_state_weights'])
SE2BeliefState.normWeights[2] = 0
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
robot.rate.sleep()
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
robot.rate.sleep()
#---------------------------------------------------------------------------
d_epsilon = 0.05 # error bound around waypoint
a_epsilon = np.deg2rad(15) # error bound for turning
# rid = 1
# opti = OptiTrackInterface()
# body = opti.trackInfo()[rid] # get data for rigid body w/ rigid body ID `rid'
# opti._close()
x, y, theta = cam.getObservation(
None) #body.z, body.x, np.deg2rad(body.yaw)
print x, y, theta
#---------------------------------------------------------------------------
def save_trajectory():
true_traj = []
traj = []
while True:
state = yield traj, true_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(
cam.getObservation(state)) #TODO: change this back
# true_traj.append(cam.getObservationPrediction(state))
# filter_time = 4 # [sec]
# filter_stab = int(filter_time/robot.dt)
# print 'filter steps:', filter_stab, 'dt:', robot.dt
path = [(x, y), (x + 1, y + 1)] #, (x+2, y+1), (x+1, y), (x, y)]
for (xs, ys), (xg, yg) in zip(path[:-1], path[1:]):
start = SE2BeliefState(xs, ys, theta)
goal = SE2BeliefState(xg, yg, np.pi /
2) # NOTE: end orientation does not matter
print 'Start:', start.conf, 'Goal:', goal.conf
tlqr = SwitchingController(robot,
cam,
goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
controller = Controller(
robot, cam, tlqr, ekf,
withNoise=True) #FIXME: withNoise option is not ok
process = save_trajectory()
process.next()
_, (trajectory, true_trajectory) = controller.execute(start, process)
robot.execute([0, 0])
print len(trajectory)
# for _ in range(filter_stab):
# ekf
# robot.execute([0, 0])
print
print 'Trajectories:'
for p, tp in zip(trajectory, true_trajectory):
print 'C:', p.conf, 'T:', tp
print
print 'Start theta:', theta
theta = trajectory[-1].conf[2]
print 'End theta:', theta
print
print
def test_unicycle_model():
isSimulation = True
if not isSimulation:
rospy.init_node("MotionModelTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
robot = UnicycleMotionModel(robotModel)
print 'Time step:', robot.dt
state = SE2BeliefState(1, 2, np.pi / 4)
control = np.array([200, 100])
print 'State Jacobian:'
print robot.getStateJacobian(state, control)
print 'Control Jacobian:'
print robot.getControlJacobian(state, control)
print 'Noise Jacobian:'
print robot.getNoiseJacobian(state, control)
print 'Noise Covariance Matrix'
print robot.processNoiseCovariance(state, control)
start = SE2BeliefState(0, 0, 0)
goal = SE2BeliefState(1, 1, np.pi / 2)
print 'Open loop path:'
policy = robot.generateOpenLoopControls(start, goal)
aux_state = start.copy()
trajectory = [start]
print 'Start:', aux_state.getXYYaw()
for u in policy:
aux_state = robot.evolve(aux_state, u)
print 'Control:', u, 'State:', aux_state.getXYYaw()
trajectory.append(aux_state)
print len(policy)
print 'Generate sample path from open loop policy'
aux_state = start.copy()
sample_path = [aux_state] # without initial uncertainty
for u in policy:
w = robot.generateNoise(aux_state, u)
aux_state = robot.evolve(aux_state, u, w)
print 'Control:', u, 'Noise:', w, 'State:', aux_state
sample_path.append(aux_state)
plt.figure()
for s, sn in zip(trajectory, sample_path):
plt.arrow(s.x,
s.y,
0.1 * np.cos(s.yaw),
0.1 * np.sin(s.yaw),
hold=True,
color='k')
plt.arrow(sn.x,
sn.y,
0.1 * np.cos(sn.yaw),
0.1 * np.sin(sn.yaw),
hold=True,
color='b')
plt.xlim([-0.3, 1.3])
plt.ylim([-0.3, 1.3])
plt.show()
print
if not isSimulation:
rospy.spin()
def test_controller__():
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
cc = mission.planning['setup']['controller']
SE2BeliefState.covNormWeight = cc['norm_covariance_weight']
SE2BeliefState.meanNormWeight = cc['norm_mean_weight']
SE2BeliefState.reachDist = cc['reach_dist']
# set the state component norm weights
SE2BeliefState.normWeights = np.array(cc['norm_state_weights'])
SE2BeliefState.normWeights[2] = 0
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
# from robots.X80Pro import X80Pro
# import time
# robot.rate.sleep()
#
# u = 0.25
# t0 = time.time()
# print 'Applying control', t0, 'control in pulse:', X80Pro.vel2pulse(u)
# # robot.execute([0, 6.5]) # TODO: rotation -> np.pi
# robot.execute([u, 0]) # linear
# print 'Control sent', time.time() - t0
# time.sleep(4-0.25)
# print 'Stop robot', time.time() - t0
# robot.execute([0, 0])
# print 'Done', time.time() - t0
# return
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
def save_trajectory():
true_traj = []
traj = []
while True:
state = yield traj, true_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(
cam.getObservation(state)) #TODO: change this back
# true_traj.append(cam.getObservationPrediction(state))
start = SE2BeliefState(0.240, 1.070, 0)
goal = SE2BeliefState(0.240, 1 + 1.070,
np.pi / 2) # NOTE: end orientation does not matter
tlqr = SwitchingController(robot,
cam,
goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
controller = Controller(robot, cam, tlqr, ekf,
withNoise=True) #FIXME: withNoise option is not ok
process = save_trajectory()
process.next()
_, (trajectory, true_trajectory) = controller.execute(start, process)
robot.execute([0, 0])
print len(trajectory)
# plt.figure()
# plt.axis('equal')
# # for lss in tlqr.lss:
# # s = lss.x
# # plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='g')
# for x, y, yaw in true_trajectory:
# plt.arrow(x, y, 0.1*np.cos(yaw), 0.1*np.sin(yaw), hold=True, color='k')
# for s in trajectory:
# plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='b')
# s = goal
# plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='g')
#
# # plt.xlim([0, 4.13])
# # plt.ylim([0, 3.54])
# plt.xlim([-0.7, 1.2])
# plt.ylim([1.1, 2.7])
# plt.show()
print
def test_controller__():
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
cc = mission.planning['setup']['controller']
SE2BeliefState.covNormWeight = cc['norm_covariance_weight']
SE2BeliefState.meanNormWeight = cc['norm_mean_weight']
SE2BeliefState.reachDist = cc['reach_dist']
# set the state component norm weights
SE2BeliefState.normWeights = np.array(cc['norm_state_weights'])
SE2BeliefState.normWeights[2] = 0
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
# from robots.X80Pro import X80Pro
# import time
# robot.rate.sleep()
#
# u = 0.25
# t0 = time.time()
# print 'Applying control', t0, 'control in pulse:', X80Pro.vel2pulse(u)
# # robot.execute([0, 6.5]) # TODO: rotation -> np.pi
# robot.execute([u, 0]) # linear
# print 'Control sent', time.time() - t0
# time.sleep(4-0.25)
# print 'Stop robot', time.time() - t0
# robot.execute([0, 0])
# print 'Done', time.time() - t0
# return
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
def save_trajectory():
true_traj = []
traj = []
while True:
state = yield traj, true_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(cam.getObservation(state)) #TODO: change this back
# true_traj.append(cam.getObservationPrediction(state))
start = SE2BeliefState(0.240, 1.070, 0)
goal = SE2BeliefState(0.240, 1 + 1.070, np.pi/2) # NOTE: end orientation does not matter
tlqr = SwitchingController(robot, cam, goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
controller = Controller(robot, cam, tlqr, ekf, withNoise=True) #FIXME: withNoise option is not ok
process = save_trajectory()
process.next()
_, (trajectory, true_trajectory) = controller.execute(start, process)
robot.execute([0, 0])
print len(trajectory)
# plt.figure()
# plt.axis('equal')
# # for lss in tlqr.lss:
# # s = lss.x
# # plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='g')
# for x, y, yaw in true_trajectory:
# plt.arrow(x, y, 0.1*np.cos(yaw), 0.1*np.sin(yaw), hold=True, color='k')
# for s in trajectory:
# plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='b')
# s = goal
# plt.arrow(s.x, s.y, 0.1*np.cos(s.yaw), 0.1*np.sin(s.yaw), hold=True, color='g')
#
# # plt.xlim([0, 4.13])
# # plt.ylim([0, 3.54])
# plt.xlim([-0.7, 1.2])
# plt.ylim([1.1, 2.7])
# plt.show()
print
def test_filter():
# from optitrack import OptiTrackInterface
from numpy.linalg import norm
import time
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
robot.rate.sleep()
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
robot.rate.sleep()
#---------------------------------------------------------------------------
x, y, theta = cam.getObservation(
None) #body.z, body.x, np.deg2rad(body.yaw)
print x, y, theta
#---------------------------------------------------------------------------
def save_trajectory():
true_traj = []
traj = []
while True:
state = yield traj, true_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(
cam.getObservation(state)) #TODO: change this back
# true_traj.append(cam.getObservationPrediction(state))
goal = SE2BeliefState(x, y, 0)
tlqr = SwitchingController(robot,
cam,
goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
updated_state = SE2BeliefState(x, y, theta, cov=np.eye(3))
print 'Before filtering', updated_state.conf, 'True:', cam.getObservation(
updated_state)
control = np.array([0, 0])
for _ in it.count():
z = cam.getObservation(updated_state)
# apply filter and get state estimate
currentLS = tlqr.getCurrentLS(0)
nextLS = tlqr.getNextLS(1)
updated_state = ekf.evolve(updated_state, control, z, currentLS,
nextLS)
print 'Filter:', updated_state.conf, 'True:', z
time.sleep(0.25)
if rospy.is_shutdown():
break
def run():
# 0. parse command-line arguments
parser = argparse.ArgumentParser(
description=
'''Script to execute an open loop trajectory on the robot. The open loop
trajectory is generated using the offline computed transition system where the
states are projected on the workspace. Also, the specification considered for
synthesis does not include constraints on the uncertainty and must be provided
the user as an LTL formula. It is the responsibility of the user to ensure that
the simplified specification is semantically close to the original GDTL formula.
''',
epilog=license_text)
parser.add_argument('mission_file', type=str, help='mission file')
parser.add_argument('ts_file', type=str, help='transition system file')
parser.add_argument('ltl_spec', type=str,
help='LTL (simplified) specification')
parser.add_argument('-o', '--output-dir', type=str, help='output directory')
parser.add_argument('-l', '--logfile', type=str, help='log file',
default='openloop.log')
parser.add_argument('-p', '--plot', action='store_true',
help='plot ground truth data')
parser.add_argument('-r', '--robot', action='store', default='ScoutPro1',
type=str,
help='select the robot to use (default: ScoutPro1)',
choices=['X80Pro1', 'X80Pro2', 'ScoutPro1', 'ScoutPro2'])
parser.add_argument('-i', '--id', action='store', default=1, type=int,
help='select the rigid body id for OptiTrack associated with the robot (default: 1)')
parser.add_argument('--debug', action='store_true', help='debug mode')
parser.add_argument('-v', '--verbose', action='store_true',
help='print/plot extra information for simulation')
# parse arguments
args = parser.parse_args()
# create output directory
if not os.path.isdir(args.output_dir):
os.makedirs(args.output_dir)
# configure logging
fs, dfs = '%(asctime)s %(levelname)s %(message)s', '%m/%d/%Y %I:%M:%S %p'
loglevel = logging.DEBUG if args.debug else logging.INFO
logging.basicConfig(filename=args.logfile, level=loglevel, format=fs, datefmt=dfs)
if args.verbose:
root = logging.getLogger()
ch = logging.StreamHandler(sys.stdout)
ch.setLevel(loglevel)
ch.setFormatter(logging.Formatter(fs, dfs))
root.addHandler(ch)
logging.info('Script arguments: %s, %s', args.robot, args.id)
fcontrols = os.path.join(args.output_dir, 'openloop_controls.txt')
ftraj = os.path.join(args.output_dir, 'openloop_traj.txt')
if os.path.isfile(fcontrols) and os.path.isfile(ftraj):
controls = np.loadtxt(fcontrols, dtype=np.float, delimiter='\t')
traj = np.loadtxt(ftraj, dtype=np.float, delimiter='\t')
else:
# 1. load mission and environment
mission = Mission.from_file(args.mission_file)
# 2. load transition system
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
motionModel = UnicycleMotionModel(robotModel, isSimulation=True)
# motionModel = OmnidirectionalMotionModel(robotModel, isSimulation=False)
ts = ts_load(args.ts_file, mission, motionModel)
# 3. compute satisfying trajectory with respect to the LTL spec
with open(args.ltl_spec, 'r') as fin:
ltl_spec = fin.readline().strip()
traj = compute_satisfying_run(ts, ltl_spec)
# 4. execute satisfying trajectory
controls = [ts.g[s][t]['control'] for s, t in it.izip(traj[:-1], traj[1:])]
logging.info('Length of control sequence: %d', len(controls))
# 5. save trajectory and control policy
np.savetxt(ftraj, traj, delimiter='\t', header=time.asctime())
np.savetxt(fcontrols, controls, delimiter='\t', header=time.asctime())
executeController(args, controls, traj)
logging.info('Done!')
def test_controller():
# from optitrack import OptiTrackInterface
from numpy.linalg import norm
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file('/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml')
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
cc = mission.planning['setup']['controller']
SE2BeliefState.covNormWeight = 0*cc['norm_covariance_weight']
SE2BeliefState.meanNormWeight = 1 #cc['norm_mean_weight']
SE2BeliefState.reachDist = cc['reach_dist']
# set the state component norm weights
SE2BeliefState.normWeights = np.array([1, 1, 0]) #cc['norm_state_weights'])
SE2BeliefState.normWeights[2] = 0
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
robot.rate.sleep()
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
robot.rate.sleep()
#---------------------------------------------------------------------------
d_epsilon=0.05 # error bound around waypoint
a_epsilon = np.deg2rad(15) # error bound for turning
# rid = 1
# opti = OptiTrackInterface()
# body = opti.trackInfo()[rid] # get data for rigid body w/ rigid body ID `rid'
# opti._close()
x, y, theta = cam.getObservation(None) #body.z, body.x, np.deg2rad(body.yaw)
print x, y, theta
#---------------------------------------------------------------------------
def save_trajectory():
true_traj = []
traj = []
while True:
state = yield traj, true_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(cam.getObservation(state)) #TODO: change this back
# true_traj.append(cam.getObservationPrediction(state))
# filter_time = 4 # [sec]
# filter_stab = int(filter_time/robot.dt)
# print 'filter steps:', filter_stab, 'dt:', robot.dt
path = [(x, y), (x+1, y+1)]#, (x+2, y+1), (x+1, y), (x, y)]
for (xs, ys), (xg, yg) in zip(path[:-1], path[1:]):
start = SE2BeliefState(xs, ys, theta)
goal = SE2BeliefState(xg, yg, np.pi/2) # NOTE: end orientation does not matter
print 'Start:', start.conf, 'Goal:', goal.conf
tlqr = SwitchingController(robot, cam, goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
controller = Controller(robot, cam, tlqr, ekf, withNoise=True) #FIXME: withNoise option is not ok
process = save_trajectory()
process.next()
_, (trajectory, true_trajectory) = controller.execute(start, process)
robot.execute([0, 0])
print len(trajectory)
# for _ in range(filter_stab):
# ekf
# robot.execute([0, 0])
print
print 'Trajectories:'
for p, tp in zip(trajectory, true_trajectory):
print 'C:', p.conf, 'T:', tp
print
print 'Start theta:', theta
theta = trajectory[-1].conf[2]
print 'End theta:', theta
print
print
def test_controller():
# from optitrack import OptiTrackInterface
from numpy.linalg import norm
import time, datetime
isSimulation = False
if not isSimulation:
rospy.init_node("ControllerTest", anonymous=True)
# 1. load mission and environment
mission = Mission.from_file(
'/home/cristi/Dropbox/work/workspace_linux/PyFIRM/src/data/simple/mission.yaml'
)
logging.info('\n' + str(mission))
logging.info('Seed: %s', mission.simulation.get('seed', None))
# load environment
env = mission.environment
# load robot model
assert mission.no_active_vehicles == 1
robotModel = mission.vehicles.itervalues().next()
print 'Motion model:', robotModel['motion_model']
cc = mission.planning['setup']['controller']
SE2BeliefState.covNormWeight = 0 * cc['norm_covariance_weight']
SE2BeliefState.meanNormWeight = 1 #cc['norm_mean_weight']
SE2BeliefState.reachDist = cc['reach_dist']
# set the state component norm weights
SE2BeliefState.normWeights = np.array([1, 1,
0]) #cc['norm_state_weights'])
SE2BeliefState.normWeights[2] = 0
robot = UnicycleMotionModel(robotModel, isSimulation)
print 'Time step:', robot.dt
robot.rate.sleep()
print 'Observation model:', robotModel['observation_model']
cam = CameraLocalization(robotModel, env, isSimulation=isSimulation)
opti = OptitrackLocalization(isSimulation=isSimulation)
quad = QuadCamPose(isSimulation=isSimulation)
robot.rate.sleep()
#---------------------------------------------------------------------------
d_epsilon = 0.05 # error bound around waypoint
a_epsilon = np.deg2rad(15) # error bound for turning
# rid = 1
# opti = OptiTrackInterface()
# body = opti.trackInfo()[rid] # get data for rigid body w/ rigid body ID `rid'
# opti._close()
x, y, theta = cam.getObservation(
None) #body.z, body.x, np.deg2rad(body.yaw)
print x, y, theta
#---------------------------------------------------------------------------
def save_trajectory():
true_traj = []
traj = []
optitrack_traj = []
quad_traj = []
while True:
state = yield traj, true_traj, optitrack_traj, quad_traj
print 'save state:', state.conf
traj.append(state.copy())
true_traj.append(
cam.getObservation(state)) #TODO: change this back
optitrack_traj.append(opti.getObservation(state))
quad_traj.append(quad.getObservation(state))
# true_traj.append(cam.getObservationPrediction(state))
filter_time = 5 # [sec]
filter_stab = int(filter_time / robot.dt)
print 'filter steps:', filter_stab, 'dt:', robot.dt
xs, ys = x, y
covs = np.eye(3)
# path = np.array([ # larger transition system
# [1.0, 2.5, 0.0],
# [1.227090539137495, 1.5261366178286393, 0],
# [0.7814101593838313, 0.6309556309870786, 0],
# [1.3450599380271273, 0.880746156908613, 0],
# [2.063849180607642, 0.9785829893820421, 0],
# [2.8594440505001724, 1.354833717086771, 0],
# [3.187859804988132, 1.976122679525189, 0],
# [3.6840077044823207, 2.8443491833405843, 0],
# [2.925239900621002, 2.52353894593588, 0],
# [2.288019681537405, 2.549151027045657, 0],
# [1.9702269162937254, 3.102135677048742, 0],
# [1.306980698970712, 3.091532230704365, 0],
# [1.0, 2.5, 0.0]
# ])
path = np.array([ # smaller transition system
# [1.0, 2.5, 0.0],
[1.227090539137495, 1.5261366178286393, 0],
[0.6683476856850583, 0.6968077181658885, 0],
[1.3618722811762958, 0.7142604793362379, 0],
[2.059680461093005, 0.9722647716781803, 0],
[2.935911437513193, 0.49039417072630315, 0],
[3.5244797253701616, 1.0671446704710397, 0],
[3.4171462977012945, 1.8636962642083317, 0],
[3.2481431339499154, 2.580396624830811, 0],
[2.6199028579323596, 2.5050497919480206, 0],
[2.143319573133728, 3.0814698610803655, 0],
[1.3410947476176465, 3.1958386839694723, 0],
[1.0, 2.5, 0.0]
] * 10)
# origin = np.array([-0.08, 0.696, 0])
origin = np.array([0, 0, 0])
path = [p - origin for p in path]
print 'Init:', path[0]
# return
# figure = plt.figure()
# figure.add_subplot('111')
# axes = figure.axes[0]
# axes.axis('equal') # sets aspect ration to 1
#
# b = np.array(env['boundary']['limits'])
# plt.xlim(b.T[0] - origin[0])
# plt.ylim(b.T[1] - origin[1])
#
# mission.draw_environment(axes, figure, origin=origin[:2])
#
# px, py, _ = np.array(path).T
#
# plt.plot(px, py, 'ko-')
#
# plt.show()
# return
#
# path = [(x, y), (x+1, y+1), (x+2, y+1), (x+1, y), (x, y)]
experiment = 70
with open('experiment_data_{}.txt'.format(experiment), 'a') as fout:
t = time.time()
print >> fout, '#', experiment, '#', t, '#', str(
datetime.datetime.fromtimestamp(t))
for xg, yg, _ in path:
start = SE2BeliefState(xs, ys, theta, cov=covs)
goal = SE2BeliefState(xg, yg, np.pi /
2) # NOTE: end orientation does not matter
print 'Start:', start.conf, 'Goal:', goal.conf
tlqr = SwitchingController(robot,
cam,
goal,
stateWeight=np.diag([1, 1, 200]),
controlWeight=np.eye(2))
ekf = ExtendedKF(cam, robot)
controller = Controller(
robot, cam, tlqr, ekf,
withNoise=True) #FIXME: withNoise option is not ok
process = save_trajectory()
process.next()
_, (trajectory, true_trajectory, opti_trajectory,
quad_traj) = controller.execute(start, process)
robot.execute([0, 0])
print len(trajectory)
print
print 'Trajectories:'
with open('experiment_data_{}.txt'.format(experiment), 'a') as fout:
for p, tp, op, qp in zip(trajectory, true_trajectory,
opti_trajectory, quad_traj):
print 'C:', p.conf, 'T:', tp, 'O:', op
print >> fout, '{', '"x": {}, "cov": {}, "filter": {}, "opti" : {}, "quad": {}'.format(
list(p.conf), list(np.diag(p.cov)), list(tp), list(op),
list(qp)), '}'
print
print >> fout
print 'Start:', xs, ys, theta
updated_state = trajectory[-1]
print 'Before filtering', updated_state.conf, 'True:', cam.getObservation(
updated_state)
for _ in range(filter_stab):
updated_state = controller.evolve(updated_state,
0,
nocontrol=True)
print >> fout, '{', '"x": {}, "cov": {}'.format(
list(updated_state.conf),
list(np.diag(updated_state.cov))), '}'
xs, ys, theta = updated_state.conf
covs = updated_state.cov
print 'End:', xs, ys, theta, 'True:', cam.getObservation(
updated_state)
print
print
print >> fout
print >> fout