def measurement_callback(self, t, dt, navdata):
from plot import plot, plot_trajectory
#plot("x_true", self.simulator.drone.x[0])
#plot("y_true", self.simulator.drone.x[1])
#plot("z_true", self.simulator.drone.x[2])
rot = self.rotation_to_world(navdata.rotZ)
updated_state = self.copy_state(self.state[-1]);
updated_state.velocity = np.dot(rot, np.array([[navdata.vx], [navdata.vy], [navdata.vz]])) + self.noise()
updated_state.position += dt * updated_state.velocity
self.state.append(updated_state);
current = self.state.pop(0)
self.update_setpoint(t)
plot_trajectory("setpoint", self.state_desired.position)
plot_trajectory("integrated", current.position)
lin_vel = self.user.compute_control_command(t, dt, current, self.copy_state(self.state_desired))
ad = current.position - self.state_desired.position
al = math.sqrt(ad.item(0)*ad.item(0) + ad.item(1)*ad.item(1) + ad.item(2)*ad.item(2))
if (al < 0.15):
self.state_desired.position[1] = 3
self.simulator.set_input_world(lin_vel, 0, self.state_desired)
def measurement_callback(self, t, dt, navdata):
import plot
rot = self.rotation_to_world(navdata.rotZ)
self.state.velocity = np.dot(rot, np.array([[navdata.vx], [navdata.vy], [navdata.vz]]))# + self.noise()
self.state.position += dt * self.state.velocity
self.update_setpoint(t)
self.kalman_state = State();
self.kalman_state.position[0:2] = self.user.x[0:2];
self.kalman_state.velocity[0:2] = self.user.x[2:4];
lin_vel = self.controller.compute_control_command(t, dt, self.kalman_state, self.state_desired) + self.noise();
self.simulator.set_input_world(lin_vel, 0)
self.user.state_callback()
vis_setpoint = self.state_desired.position;
vis_setpoint[2] += 0.01
plot.plot_trajectory("setpoint", vis_setpoint);
if(self.counter % 15 == 0):
measurement = self.state.position + self.noise2();
self.user.measurement_callback(measurement[0:2])
self.counter +=1
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# Translation coordinates x and y
posX = np.multiply(navdata.vx, dt)
posY = np.multiply(navdata.vy, dt)
# Rotation about Z axis
rotPsi = navdata.rotZ
# Matrix of Quadcopter's local position
localPos = np.array([[posX],
[posY]])
# Transformation matrix to transform local to global coordinates
rotMatrix = np.array([[cos(rotPsi), -sin(rotPsi)],
[sin(rotPsi), cos(rotPsi)]])
# Transform from local to global
globalPos = np.dot(rotMatrix, localPos)
# Quadcopter Global coordinates position update
self.position += globalPos
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# Translation coordinates x and y
posX = np.multiply(navdata.vx, dt)
posY = np.multiply(navdata.vy, dt)
# Rotation about Z axis
rotPsi = navdata.rotZ
# Matrix of Quadcopter's local position
localPos = np.array([[posX], [posY]])
# Transformation matrix to transform local to global coordinates
rotMatrix = np.array([[cos(rotPsi), -sin(rotPsi)],
[sin(rotPsi), cos(rotPsi)]])
# Transform from local to global
globalPos = np.dot(rotMatrix, localPos)
# Quadcopter Global coordinates position update
self.position += globalPos
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
import plot
rot = self.rotation_to_world(navdata.rotZ)
self.state.velocity = np.dot(rot,
np.array([[navdata.vx], [navdata.vy],
[navdata.vz]
])) # + self.noise()
self.state.position += dt * self.state.velocity
self.update_setpoint(t)
self.kalman_state = State()
self.kalman_state.position[0:2] = self.user.x[0:2]
self.kalman_state.velocity[0:2] = self.user.x[2:4]
lin_vel = self.controller.compute_control_command(
t, dt, self.kalman_state, self.state_desired) + self.noise()
self.simulator.set_input_world(lin_vel, 0)
self.user.state_callback()
vis_setpoint = self.state_desired.position
vis_setpoint[2] += 0.01
plot.plot_trajectory("setpoint", vis_setpoint)
if (self.counter % 15 == 0):
measurement = self.state.position + self.noise2()
self.user.measurement_callback(measurement[0:2])
self.counter += 1
def state_callback(self):
self.x = self.predictState(self.A, self.x)
self.sigma = self.predictCovariance(self.A, self.sigma, self.Q)
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2,0:2])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
#self.position[0] += cos(navdata.rotZ) * navdata.vx * dt - sin(navdata.rotZ) * navdata.vy * dt
#self.position[1] += sin(navdata.rotZ) * navdata.vx * dt + cos(navdata.rotZ) * navdata.vy * dt
cs = cos(navdata.rotZ)
sn = sin(navdata.rotZ)
R = np.array([[cs, -sn], [sn, cs]])
v = np.array([[navdata.vx, navdata.vy]]).transpose()
move = np.dot(R, v) * dt
self.position += move
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
#Navdata
#The navdata contains the following values:
#
#rotX - roll angle in radians (rotation around x axis)
#rotY - pitch angle in radians (rotation around y axis)
#rotZ - yaw angle in radians (rotation around z axis)
#vx - velocity along the x axis (local)
#vy - velocity along the y axis (local)
def state_callback(self):
self.x = self.predictState(self.A, self.x)
self.sigma = self.predictCovariance(self.A, self.sigma, self.Q)
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, t, dt, navdata):
# t: time since simulation start
# dt : time since last call to measurement_callback
# navdata : measurements of the quadrotor
self.position = self.position + dt * np.dot(self.rotation_to_world(navdata.rotZ),
np.array([[navdata.vx], [navdata.vy]]))
plot_trajectory("odometry", self.position)
def measurement_callback(self, marker_position_world, marker_yaw_world,
marker_position_relative, marker_yaw_relative):
'''
:param measurement: vector of measured coordinates
'''
while (marker_yaw_relative > math.pi):
marker_yaw_relative -= 2 * math.pi
while (marker_yaw_relative < -math.pi):
marker_yaw_relative += 2 * math.pi
from math import sin, cos
#meas = Pose2D(marker_yaw_world, marker_position_world) * Pose2D(marker_yaw_relative, marker_position_relative).inv();
measurement = np.array([[
marker_position_relative[0], marker_position_relative[1],
marker_yaw_relative
]]).T
s_psi = sin(self.x[2])
c_psi = cos(self.x[2])
#TODO: Enter the Jacobian derived in the previous assignment
dx = marker_position_world[0] - self.x[0]
dy = marker_position_world[1] - self.x[1]
self.H = np.array([[-c_psi, -s_psi, s_psi * dx - c_psi * dy],
[s_psi, -c_psi, c_psi * dx + s_psi * dy],
[0, 0, -1]])
I = np.zeros((3, 3))
self.H = np.hstack((self.H, I))
predicted_meas = Pose2D(self.x[2], self.x[0:2]).inv() * Pose2D(
marker_yaw_world, marker_position_world)
predicted_measurement = np.array([[
predicted_meas.translation[0], predicted_meas.translation[1],
predicted_meas.yaw()
]]).T
while (predicted_measurement[2] > math.pi):
predicted_measurement[2] -= 2 * math.pi
while (predicted_measurement[2] < -math.pi):
predicted_measurement[2] += 2 * math.pi
print "z_predicted", predicted_measurement.T
print "z", measurement.T
# visualize measurement
plot_point("gps", measurement[0:2])
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
#print "kalman", k
#print "before", self.x.T
self.x = self.correctState(measurement, self.x, k,
predicted_measurement)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
#print "after", self.x.T
# visualize position state
p = self.x[0:2]
plot_trajectory("kalman", p)
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
self.position = self.position + dt * np.dot(self.rotation_to_world(navdata.rotZ), np.array([[navdata.vx], [navdata.vy]]))
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
self.position += dt * np.dot(self.rotation_to_world(navdata.rotZ), np.array([[navdata.vx], [navdata.vy]]))
plot_trajectory("odometry", self.position)
def main():
args = parse_args()
header, counts = read_counts(args.counts)
counts = pd.DataFrame(counts, columns=header)
counts = sort_counts(counts)
num_genes = int(args.num_genes)-1
out_dir = args.out_dir
# plot pca
print('Generating PCA plot ...')
plot.plot_pca(out_dir, counts)
# plot heatmap
print('Generating Gene Expression Heatmap ...')
plot.plot_heatmap(out_dir, counts, num_genes)
# Plot gene expression tragectory
print('Generating Gene Expression Trajectory ...')
plot.plot_trajectory(out_dir, counts, num_genes)
# Save formatted and sorted count file
counts.to_csv('data/counts_clust.txt', index=False, sep='\t')
# Call clust to cluster the genes
print('Clustering Genes (this might take awhile) ...')
subprocess.call(['mkdir', 'clust_out'])
if args.kmeans is not None:
subprocess.call(['clust', 'data/counts_clust.txt',
'-o', './clust_out', '-K', args.kmeans])
else:
subprocess.call(['clust', 'data/counts_clust.txt',
'-o', './clust_out'])
# Make sure everythin is all set for motif enrichment
clust_out = args.input_genes
ref_bed = args.bedfile
ref_fa = args.genome
ref_motif = args.motif
try:
open(clust_out)
except FileNotFoundError:
print('Clustering failed, '
+ 'Clusters_Objects.tsv not found in the output directory')
print('Motif Enrichment Analysis (this might take awhile) ...')
try:
enrichment.run_motif_enrichment(clust_out,
ref_bed,
ref_fa, ref_motif,
args.ame,
100, 100)
except NameError:
print('AME is not installed! Please install AME and try again')
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
cur_mat = rot_trans_mat(self.position[0], self.position[1], navdata.rotZ)
new_pos = dot(cur_mat, array([[navdata.vx * dt], [navdata.vy * dt], [1.0]]))
#print new_pos
self.position = array([[new_pos[0]], [new_pos[1]]])
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
import plot
from math import hypot
yaw_vel = (navdata.rotZ - self.last_yaw) / dt
self.last_yaw = navdata.rotZ
noise_lin_vel = np.random.normal(0.0, 0.1, (2, 1))
noise_yaw_vel = np.random.normal(0.0, 0.1, (1, 1))
u = self.user.state_callback(t, dt, np.array([[navdata.vx, navdata.vy]]).T + noise_lin_vel, yaw_vel + noise_yaw_vel[0])
#for beacon in self.user.beacons: #should the beacons be in user or internal? the grading safely happens in javascript right?
# if(hypot(beacon.position[0] - self.simulator.drone.x[0], beacon.position[1] - self.simulator.drone.x[1]) <= 0.5):
# self.user.beacon_callback(beacon)
if(self.counter % 7 == 0): # approx. 30Hz
for i in range(len(self.markers)):
visible, rel_pos, rel_yaw = self.is_marker_visible(self.markers[i])
if visible:
plot.plot_trajectory("test", self.simulator.drone.x);
plot.plot_trajectory("test", np.dot(self.rotation_to_world(self.simulator.drone.theta[2]), rel_pos) + self.simulator.drone.x);
noise_pos = np.random.normal(0, 0.005, (2, 1));
noise_yaw = np.random.normal(0, 0.03, (1, 1));
marker_pos_world = np.array([[self.markers[i][0], self.markers[i][1]]]).T
marker_pos_quadrotor = rel_pos[0:2] + noise_pos
marker_yaw_world = self.markers[i][2]
marker_yaw_quadrotor = rel_yaw + noise_yaw[0]
self.user.measurement_callback(marker_pos_world, marker_yaw_world, marker_pos_quadrotor, marker_yaw_quadrotor)
break;
self.counter += 1
lin_vel = np.zeros((2,1))
yaw_vel = 0
if type(u) is tuple and len(u) == 2:
noise_lin_vel = np.random.normal(0.0, 0.15, (2, 1))
noise_yaw_vel = np.random.normal(0.1, 0.1, (1, 1))
lin_vel = u[0] + noise_lin_vel
yaw_vel = u[1] + noise_yaw_vel[0]
else:
print "state_callback(...) should return a tuple of linear velocity control command (2x1 numpy.array) and yaw velocity command (float)!"
u_internal = [lin_vel[0], lin_vel[1], yaw_vel, 0];
self.simulator.set_input(u_internal)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
distance = navdata.vx * dt
dx = distance * cos(navdata.rotZ)
dy = distance * sin(navdata.rotZ)
self.position = self.position + np.array([[dx], [dy]])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
cur_mat = rot_trans_mat(self.position[0], self.position[1],
navdata.rotZ)
new_pos = dot(cur_mat,
array([[navdata.vx * dt], [navdata.vy * dt], [1.0]]))
#print new_pos
self.position = array([[new_pos[0]], [new_pos[1]]])
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
distance = navdata.vx*dt
dx = distance*cos(navdata.rotZ)
dy = distance*sin(navdata.rotZ)
self.position = self.position + np.array([[dx],[dy]])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
d = navdata.vx * dt
a = navdata.rotZ
t = np.array([[d * math.cos(a)], [d * math.sin(a)]])
self.position = self.position + t
# update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, measurement):
'''
param measurement: vector of measured coordinates
'''
# Visualize measurement
plot_point("gps", measurement)
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
self.x = self.correctState(measurement, self.x, k, self.H)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
# Visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
local_velocity = np.array([[navdata.vx], [navdata.vy]])
prev_position = self.position
angle = navdata.rotZ
rotation_to_global = np.array([[cos(angle), -sin(angle)],
[sin(angle), cos(angle)]])
velocity = np.array([[navdata.vx], [navdata.vy]])
self.position += dt * np.dot(rotation_to_global, velocity)
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, measurement):
'''
:param measurement: vector of measured coordinates
'''
# visualize measurement
plot_point("gps", measurement)
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
self.x = self.correctState(measurement, self.x, k, self.H)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2,0:2])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
#mat = np.array([[cos(navdata.rotZ), -sin(navdata.rotZ), navdata.vx], [sin(navdata.rotZ), cos(navdata.rotZ), navdata.vy], [0, 0, 1]])
#vec = mat * np.array([self.position[0,0], self.position[1, 0], 0])
#self.position = np.array([[vec[0,0]], [vec[1, 0]]])
pos_local=np.array([[navdata.vx*dt],[navdata.vy*dt]])
R=np.array([[cos(navdata.rotZ),-1.0*sin(navdata.rotZ)],[sin(navdata.rotZ),cos(navdata.rotZ)]])
pos_global=np.dot(R,pos_local)
self.position +=pos_global
plot_trajectory("odometry", self.position)
def main():
# common style arguments for plotting
style = {
'border': {
'color': 'red',
'linewidth': 0.5
},
}
# set-up mdp
world, reward, terminal = setup_mdp()
# show our original reward
ax = plt.figure(num='Original Reward').add_subplot(111)
P.plot_state_values(ax, world, reward, **style)
plt.draw()
# generate "expert" trajectories
trajectories, expert_policy = generate_trajectories(
world, reward, terminal)
# show our expert policies
ax = plt.figure(num='Expert Trajectories and Policy').add_subplot(111)
P.plot_stochastic_policy(ax, world, expert_policy, **style)
for t in trajectories:
P.plot_trajectory(ax, world, t, lw=5, color='white', alpha=0.025)
plt.draw()
# maximum entropy reinforcement learning (non-causal)
reward_maxent = maxent(world, terminal, trajectories)
# show the computed reward
ax = plt.figure(num='MaxEnt Reward').add_subplot(111)
P.plot_state_values(ax, world, reward_maxent, **style)
plt.draw()
# maximum casal entropy reinforcement learning (non-causal)
reward_maxcausal = maxent_causal(world, terminal, trajectories)
# show the computed reward
ax = plt.figure(num='MaxEnt Reward (Causal)').add_subplot(111)
P.plot_state_values(ax, world, reward_maxcausal, **style)
plt.draw()
plt.show()
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
R = np.array([[cos(navdata.rotZ),-1*sin(navdata.rotZ),self.position[0]],
[sin(navdata.rotZ),cos(navdata.rotZ),self.position[1]],
[0,0,1]]);
x = navdata.vx * dt;
y = navdata.vy * dt;
P_global = np.dot(R, np.array([[x],[y],[1]]));
self.position = np.array([[P_global[0]],[P_global[1]]]);
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# TODO: update self.position by integrating measurements contained in navdata
R = np.array(
[[cos(navdata.rotZ), -1 * sin(navdata.rotZ), self.position[0]],
[sin(navdata.rotZ),
cos(navdata.rotZ), self.position[1]], [0, 0, 1]])
x = navdata.vx * dt
y = navdata.vy * dt
P_global = np.dot(R, np.array([[x], [y], [1]]))
self.position = np.array([[P_global[0]], [P_global[1]]])
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
:The navdata contains the following values:
:rotX - roll angle in radians (rotation around x axis)
:rotY - pitch angle in radians (rotation around y axis)
:rotZ - yaw angle in radians (rotation around z axis)
:vx - velocity along the x axis
:vy - velocity along the y axis
'''
# rotZ gives us the absolute angle around the z axis
# positive is in the counter clockwise direction
self.position[0] += navdata.vx * dt * cos(navdata.rotZ)
self.position[1] += navdata.vx * dt * sin(navdata.rotZ)
plot_trajectory("odometry", self.position)
def state_callback(self, dt, linear_velocity, yaw_velocity):
from math import sin, cos
self.x[3:5] = linear_velocity
self.x[5] = yaw_velocity
#print yaw_velocity
self.A[0:2, 3:5] = np.array([[cos(self.x[2]), -sin(self.x[2])],
[sin(self.x[2]),
cos(self.x[2])]]) * dt
self.A[3:5, 3:5] = np.array([[cos(self.x[2]), -sin(self.x[2])],
[sin(self.x[2]),
cos(self.x[2])]])
self.x = self.predictState(self.A, self.x)
self.sigma = self.predictCovariance(self.A, self.sigma, self.Q)
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
:The navdata contains the following values:
:rotX - roll angle in radians (rotation around x axis)
:rotY - pitch angle in radians (rotation around y axis)
:rotZ - yaw angle in radians (rotation around z axis)
:vx - velocity along the x axis
:vy - velocity along the y axis
'''
# rotZ gives us the absolute angle around the z axis
# positive is in the counter clockwise direction
self.position[0] += navdata.vx*dt*cos(navdata.rotZ)
self.position[1] += navdata.vx*dt*sin(navdata.rotZ)
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
# compute transformation matrix
psi = navdata.rotZ
R = np.array([[cos(psi), -sin(psi)], [sin(psi), cos(psi)]])
vx = navdata.vx
vy = navdata.vy
ds_local = np.array([[vx * dt],[vy * dt]])
ds_global = np.dot(R, ds_local)
self.position = self.position + ds_global
# TODO: update self.position by integrating measurements contained in navdata
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
from plot import plot, plot_trajectory
#plot("x_true", self.simulator.drone.x[0])
#plot("y_true", self.simulator.drone.x[1])
#plot("z_true", self.simulator.drone.x[2])
rot = self.rotation_to_world(navdata.rotZ)
updated_state = self.copy_state(self.state[-1]);
updated_state.velocity = np.dot(rot, np.array([[navdata.vx], [navdata.vy], [navdata.vz]])) + self.noise()
updated_state.position += dt * updated_state.velocity
self.state.append(updated_state);
current = self.state.pop(0)
self.update_setpoint(t)
plot_trajectory("setpoint", self.state_desired.position)
plot_trajectory("integrated", current.position)
lin_vel = self.user.compute_control_command(t, dt, current, self.copy_state(self.state_desired))
self.simulator.set_input_world(lin_vel, 0)
def measurement_callback(self, t, dt, navdata):
'''
:param t: time since simulation start
:param dt: time since last call to measurement_callback
:param navdata: measurements of the quadrotor
'''
#Rotation matrix using homogeneous coordinates
R = np.array([[cos(navdata.rotZ), sin(navdata.rotZ)*-1, self.position[0]], [sin(navdata.rotZ), cos(navdata.rotZ), self.position[1]], [0,0,1]]);
#Position vector in local coordinates (homogeneous)
localPos = np.array([[navdata.vx*dt], [navdata.vy*dt], [1]]);
#Multiply to transform into global coordinates
globalPos = np.dot(R, localPos);
self.position[0] = globalPos[0];
self.position[1] = globalPos[1];
plot_trajectory("odometry", self.position)
def measurement_callback(self, t, dt, navdata):
import plot
rot = self.rotation_to_world(navdata.rotZ)
self.state.lin_velocity = np.dot(rot, np.array([[navdata.vx], [navdata.vy], [navdata.vz]]))
self.state.position += dt * self.state.lin_velocity
self.state.orientation = np.array([[navdata.rotX], [navdata.rotY], [navdata.rotZ]])
self.update_setpoint(t)
yaw_vel = (navdata.rotZ - self.last_yaw) / dt
self.last_yaw = navdata.rotZ
noise_lin_vel = np.random.normal(0.0, 0.5, (2, 1))
noise_yaw_vel = np.random.normal(0.05, 0.5, (1, 1))
self.user.state_callback(t, dt, np.array([[navdata.vx, navdata.vy]]).T + noise_lin_vel, yaw_vel + noise_yaw_vel[0])
vis_setpoint = self.state_desired.position;
vis_setpoint[2] += 0.01
plot.plot_trajectory("setpoint", vis_setpoint);
if(self.counter % 7 == 0): # approx. 30Hz
for i in range(len(self.markers)):
visible, rel_pos, rel_yaw = self.is_marker_visible(self.markers[i], self.state)
if visible:
plot.plot_trajectory("test", self.state.position);
plot.plot_trajectory("test", np.dot(self.rotation_to_world(self.state.orientation[2]), rel_pos) + self.state.position);
noise_pos = np.random.normal(0, 0.005, (2, 1));
noise_yaw = np.random.normal(0, 0.03, (1, 1));
marker_pos_world = np.array([[self.markers[i][0], self.markers[i][1]]]).T
marker_pos_quadrotor = rel_pos[0:2] + noise_pos
marker_yaw_world = self.markers[i][2]
marker_yaw_quadrotor = rel_yaw + noise_yaw[0]
self.user.measurement_callback(marker_pos_world, marker_yaw_world, marker_pos_quadrotor, marker_yaw_quadrotor)
break;
self.counter +=1
self.kalman_state = State();
self.kalman_state.position[0:2] = self.user.x[0:2]; #xy
self.kalman_state.position[2] = self.state.position[2]; #z
self.kalman_state.orientation[2] = self.user.x[2]; #psi
#self.kalman_state.lin_velocity[0:2] = self.user.x[3:5]; #xdot,ydot
#self.kalman_state.ang_velocity[2] = self.user.x[5]; #psidot
#print "est yaw: ", self.kalman_state.orientation[2]
tmp = navdata.rotZ
while(tmp > math.pi):
tmp -= 2 * math.pi
while(tmp < -math.pi):
tmp += 2 * math.pi
#print "act yaw: ", tmp
lin_vel, ang_vel = self.controller.compute_control_command(t, dt, self.kalman_state, self.state_desired) + self.noise();
self.simulator.set_input_world(lin_vel, ang_vel)
def mce_irl(grid_size, p_slip, avoid_states):
cwd = os.getcwd()
fig_dir = "figs"
if not os.path.exists(fig_dir):
os.makedirs(fig_dir)
# common style arguments for plotting
style = {
'border': {'color': 'red', 'linewidth': 0.5},
'cmap': "Blues",
}
# set-up mdp
world, reward, terminal = setup_mdp(grid_size, p_slip, avoid_states)
# show our original reward
ax = plt.figure(num='Original Reward').add_subplot(111)
tt = P.plot_state_values(ax, world, reward, **style)
plt.colorbar(tt)
plt.draw()
plt.savefig(os.path.join(fig_dir, "gt_reward.png"))
print("\nGenerating expert trajectories ...\n")
# generate "expert" trajectories
trajectories, expert_policy = generate_trajectories(world, reward, terminal)
# show our expert policies
ax = plt.figure(num='Expert Trajectories and Policy').add_subplot(111)
P.plot_stochastic_policy(ax, world, expert_policy, **style)
for t in trajectories:
P.plot_trajectory(ax, world, t, lw=5, color='white', alpha=0.025)
plt.draw()
plt.savefig(os.path.join(fig_dir, "demonstrations.png"))
'''
print("ME-IRL ...")
# maximum entropy reinforcement learning (non-causal)
reward_maxent = maxent(world, terminal, trajectories)
# show the computed reward
ax = plt.figure(num='MaxEnt Reward').add_subplot(111)
P.plot_state_values(ax, world, reward_maxent, **style)
plt.draw()
'''
print("\nPerforming MCE-IRL ...\n")
# maximum casal entropy reinforcement learning (non-causal)
reward_maxcausal = maxent_causal(world, terminal, trajectories)
# print(reward_maxcausal)
# show the computed reward
ax = plt.figure(num='MaxEnt Reward (Causal)').add_subplot(111)
tt = P.plot_state_values(ax, world, reward_maxcausal, **style)
plt.colorbar(tt)
plt.draw()
plt.savefig(os.path.join(fig_dir, "maxent_causal_reward.png"))
print("\nDone! Rewards learned\n")
# plt.show()
return (reward_maxcausal, world, terminal)
def visualizeState(self):
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2,0:2])
def visualizeState(self):
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def simulate_step(self, t, dt):
self.step_count += 1
#if(int(t / 8.0) % 2 == 0):
# self.xdot_desired[0] = 0.75;
# self.xdot_desired[1] = 0.1;
#else:
# self.xdot_desired[0] = -0.75;
# self.xdot_desired[1] = -0.1;
#self.xdot_desired = np.dot(self.drone.yaw_rotation(), self.xdot_desired)
#inputCurrents = self.controller.calculate_control_command(dt, self.theta_desired, self.thetadot_desired,self.x_desired,self.xdot_desired)
inputCurrents, acc = self.controller.calculate_control_command3(dt, self.xdot_desired, self.thetadot_desired.item(2), self.theta_desired, self.x_desired)
omega = self.drone.omega;#thetadot_in_body() # calculate current angular velocity in body frame
#torques_thrust = self.drone.torques_thrust(np.array([inputCurrents]).transpose())
torques_thrust = self.drone.torques_thrust(inputCurrents)
#print acc.transpose();
# print omega.transpose()
linear_acceleration = self.linear_acceleration(torques_thrust[3], self.drone.theta, self.drone.xdot) # calculate the resulting linear acceleration
omegadot = self.angular_acceleration(torques_thrust[0:3,0], omega) # calculate resulting angular acceleration
#print "angular acc", omegadot.transpose()
#linear_acceleration = self.linear_acceleration2(inputCurrents, self.drone.theta, self.drone.xdot) # calculate the resulting linear acceleration
#omegadot = self.angular_acceleration2(inputCurrents, omega) # calculate resulting angular acceleration
omega = omega + dt * omegadot # integrate up new angular velocity in the body frame
#print "inputs:", inputCurrents
#print "omega:", omega.transpose(), "omegadot:", omegadot.transpose()
self.drone.omega = omega;
self.drone.thetadot = np.dot(self.drone.angle_rotation_to_world().transpose(), omega) # calculate roll, pitch, yaw velocities
self.drone.theta = self.drone.theta + dt * self.drone.thetadot # calculate new roll, pitch, yaw angles
#print self.drone.xdot.transpose(), self.drone.theta.transpose(), omega.transpose(), omegadot.transpose()
#print "d", inputCurrents
#print "a", torques_thrust[0:3,0], "b", omega, "c", omegadot
#print "thetadot:",self.drone.thetadot
#print("New theta",self.drone.theta)
self.drone.xdoubledot = linear_acceleration
self.drone.xdot = self.drone.xdot + dt * linear_acceleration # calculate new linear drone speed
self.drone.x = self.drone.x + dt * self.drone.xdot # calculate new drone position
#print "acc", linear_acceleration
#print "theta", self.drone.theta
#print("Position",self.drone.x.transpose())
if(sys.platform == "skulpt"):
import plot
plot.plot_pose("ardrone", self.drone.x, self.drone.theta)
plot.plot_trajectory("ardrone", self.drone.x)
plot.plot_motor_command("ardrone", inputCurrents)
elif(self.step_count % 50 == 0): #save trajectory for plotting
vel = np.dot(self.rotation(self.drone.theta).transpose(), self.drone.xdot)
vel = self.drone.xdot
#vel = self.drone.x
#vel = acc
#vel = self.drone.thetadot_in_body();
#vel = self.drone.xdot;
self.x.append(vel.item(0))
self.y.append(vel.item(1))
self.z.append(vel.item(2))
ang = self.drone.theta
self.roll.append( ang.item(0))
self.pitch.append(ang.item(1))
self.yaw.append( ang.item(2))
#print self.theta_desired.item(2)
self.cmd1.append(inputCurrents[0] - inputCurrents[2])
self.cmd2.append(inputCurrents[1] - inputCurrents[3])
self.cmd3.append(inputCurrents[0] - inputCurrents[1])
self.cmd4.append(inputCurrents[3])
self.roll_des.append(self.theta_desired[0])
self.pitch_des.append(self.theta_desired[1])
self.yaw_des.append(self.theta_desired[2])
def measurement_callback(self, t, dt, navdata):
import plot
rot = self.rotation_to_world(navdata.rotZ)
self.state.lin_velocity = np.dot(
rot, np.array([[navdata.vx], [navdata.vy], [navdata.vz]]))
self.state.position += dt * self.state.lin_velocity
self.state.orientation = np.array([[navdata.rotX], [navdata.rotY],
[navdata.rotZ]])
self.update_setpoint(t)
yaw_vel = (navdata.rotZ - self.last_yaw) / dt
self.last_yaw = navdata.rotZ
noise_lin_vel = np.random.normal(0.0, 0.5, (2, 1))
noise_yaw_vel = np.random.normal(0.05, 0.5, (1, 1))
self.user.state_callback(
t, dt,
np.array([[navdata.vx, navdata.vy]]).T + noise_lin_vel,
yaw_vel + noise_yaw_vel[0])
vis_setpoint = self.state_desired.position
vis_setpoint[2] += 0.01
plot.plot_trajectory("setpoint", vis_setpoint)
if (self.counter % 7 == 0): # approx. 30Hz
for i in range(len(self.markers)):
visible, rel_pos, rel_yaw = self.is_marker_visible(
self.markers[i], self.state)
if visible:
plot.plot_trajectory("test", self.state.position)
plot.plot_trajectory(
"test",
np.dot(
self.rotation_to_world(self.state.orientation[2]),
rel_pos) + self.state.position)
noise_pos = np.random.normal(0, 0.005, (2, 1))
noise_yaw = np.random.normal(0, 0.03, (1, 1))
marker_pos_world = np.array(
[[self.markers[i][0], self.markers[i][1]]]).T
marker_pos_quadrotor = rel_pos[0:2] + noise_pos
marker_yaw_world = self.markers[i][2]
marker_yaw_quadrotor = rel_yaw + noise_yaw[0]
self.user.measurement_callback(marker_pos_world,
marker_yaw_world,
marker_pos_quadrotor,
marker_yaw_quadrotor)
break
self.counter += 1
self.kalman_state = State()
self.kalman_state.position[0:2] = self.user.x[0:2]
#xy
self.kalman_state.position[2] = self.state.position[2]
#z
self.kalman_state.orientation[2] = self.user.x[2]
#psi
#self.kalman_state.lin_velocity[0:2] = self.user.x[3:5]; #xdot,ydot
#self.kalman_state.ang_velocity[2] = self.user.x[5]; #psidot
#print "est yaw: ", self.kalman_state.orientation[2]
tmp = navdata.rotZ
while (tmp > math.pi):
tmp -= 2 * math.pi
while (tmp < -math.pi):
tmp += 2 * math.pi
#print "act yaw: ", tmp
lin_vel, ang_vel = self.controller.compute_control_command(
t, dt, self.kalman_state, self.state_desired) + self.noise()
self.simulator.set_input_world(lin_vel, ang_vel)
def measurement_callback(self, t, dt, navdata):
import plot
from math import hypot
yaw_vel = (navdata.rotZ - self.last_yaw) / dt
self.last_yaw = navdata.rotZ
noise_lin_vel = np.random.normal(0.0, 0.1, (2, 1))
noise_yaw_vel = np.random.normal(0.0, 0.1, (1, 1))
u = self.user.state_callback(
t, dt,
np.array([[navdata.vx, navdata.vy]]).T + noise_lin_vel,
yaw_vel + noise_yaw_vel[0])
#for beacon in self.user.beacons: #should the beacons be in user or internal? the grading safely happens in javascript right?
# if(hypot(beacon.position[0] - self.simulator.drone.x[0], beacon.position[1] - self.simulator.drone.x[1]) <= 0.5):
# self.user.beacon_callback(beacon)
if (self.counter % 7 == 0): # approx. 30Hz
for i in range(len(self.markers)):
visible, rel_pos, rel_yaw = self.is_marker_visible(
self.markers[i])
if visible:
plot.plot_trajectory("test", self.simulator.drone.x)
plot.plot_trajectory(
"test",
np.dot(
self.rotation_to_world(
self.simulator.drone.theta[2]), rel_pos) +
self.simulator.drone.x)
noise_pos = np.random.normal(0, 0.005, (2, 1))
noise_yaw = np.random.normal(0, 0.03, (1, 1))
marker_pos_world = np.array(
[[self.markers[i][0], self.markers[i][1]]]).T
marker_pos_quadrotor = rel_pos[0:2] + noise_pos
marker_yaw_world = self.markers[i][2]
marker_yaw_quadrotor = rel_yaw + noise_yaw[0]
self.user.measurement_callback(marker_pos_world,
marker_yaw_world,
marker_pos_quadrotor,
marker_yaw_quadrotor)
break
self.counter += 1
lin_vel = np.zeros((2, 1))
yaw_vel = 0
if type(u) is tuple and len(u) == 2:
noise_lin_vel = np.random.normal(0.0, 0.15, (2, 1))
noise_yaw_vel = np.random.normal(0.1, 0.1, (1, 1))
lin_vel = u[0] + noise_lin_vel
yaw_vel = u[1] + noise_yaw_vel[0]
else:
print "state_callback(...) should return a tuple of linear velocity control command (2x1 numpy.array) and yaw velocity command (float)!"
u_internal = [lin_vel[0], lin_vel[1], yaw_vel, 0]
self.simulator.set_input(u_internal)
###############################################################################
## Plotting
method_dirs = ['curvature', 'compromise', 'laptime', 'sectors', 'estimated']
plot_dir = os.path.join(
os.path.dirname(__file__), '..', 'data', 'plots', track.name,
method_dirs[args.method]
)
if not os.path.exists(plot_dir): os.makedirs(plot_dir)
if args.plot_corners or args.plot_all:
plot_corners(
os.path.join(plot_dir, "corners." + args.ext),
track.left, track.right, track.mid.position(trajectory.s),
track.corners(trajectory.s, K_MIN, PROXIMITY, LENGTH)[1]
)
if args.plot_path or args.plot_all:
plot_path(
os.path.join(plot_dir, "path." + args.ext),
track.left, track.right, trajectory.path.position(trajectory.s),
trajectory.path.controls
)
if args.plot_trajectory or args.plot_all:
plot_trajectory(
os.path.join(plot_dir, "trajectory." + args.ext),
track.left, track.right, trajectory.path.position(trajectory.s),
trajectory.velocity.v
)
def simulate_step(self, t, dt):
self.step_count += 1
#if(int(t / 8.0) % 2 == 0):
# self.xdot_desired[0] = 0.75;
# self.xdot_desired[1] = 0.1;
#else:
# self.xdot_desired[0] = -0.75;
# self.xdot_desired[1] = -0.1;
#self.xdot_desired = np.dot(self.drone.yaw_rotation(), self.xdot_desired)
#inputCurrents = self.controller.calculate_control_command(dt, self.theta_desired, self.thetadot_desired,self.x_desired,self.xdot_desired)
inputCurrents, acc = self.controller.calculate_control_command3(
dt, self.xdot_desired, self.thetadot_desired.item(2))
omega = self.drone.omega
#thetadot_in_body() # calculate current angular velocity in body frame
#torques_thrust = self.drone.torques_thrust(np.array([inputCurrents]).transpose())
torques_thrust = self.drone.torques_thrust(inputCurrents)
#print acc.transpose();
# print omega.transpose()
linear_acceleration = self.linear_acceleration(
torques_thrust[3], self.drone.theta,
self.drone.xdot) # calculate the resulting linear acceleration
omegadot = self.angular_acceleration(
torques_thrust[0:3, 0],
omega) # calculate resulting angular acceleration
#print "angular acc", omegadot.transpose()
#linear_acceleration = self.linear_acceleration2(inputCurrents, self.drone.theta, self.drone.xdot) # calculate the resulting linear acceleration
#omegadot = self.angular_acceleration2(inputCurrents, omega) # calculate resulting angular acceleration
omega = omega + dt * omegadot # integrate up new angular velocity in the body frame
#print "inputs:", inputCurrents
#print "omega:", omega.transpose(), "omegadot:", omegadot.transpose()
self.drone.omega = omega
self.drone.thetadot = np.dot(
self.drone.angle_rotation_to_world().transpose(),
omega) # calculate roll, pitch, yaw velocities
self.drone.theta = self.drone.theta + dt * self.drone.thetadot # calculate new roll, pitch, yaw angles
#print self.drone.xdot.transpose(), self.drone.theta.transpose(), omega.transpose(), omegadot.transpose()
#print "d", inputCurrents
#print "a", torques_thrust[0:3,0], "b", omega, "c", omegadot
#print "thetadot:",self.drone.thetadot
#print("New theta",self.drone.theta)
self.drone.xdoubledot = linear_acceleration
self.drone.xdot = self.drone.xdot + dt * linear_acceleration # calculate new linear drone speed
self.drone.x = self.drone.x + dt * self.drone.xdot # calculate new drone position
#print "acc", linear_acceleration
#print "theta", self.drone.theta
#print("Position",self.drone.x.transpose())
if (sys.platform == "skulpt"):
import plot
plot.plot_pose("ardrone", self.drone.x, self.drone.theta)
plot.plot_trajectory("ardrone", self.drone.x)
plot.plot_motor_command("ardrone", inputCurrents)
elif (self.step_count % 50 == 0): #save trajectory for plotting
vel = np.dot(
self.rotation(self.drone.theta).transpose(), self.drone.xdot)
vel = self.drone.xdot
#vel = self.drone.x
#vel = acc
#vel = self.drone.thetadot_in_body();
#vel = self.drone.xdot;
self.x.append(vel.item(0))
self.y.append(vel.item(1))
self.z.append(vel.item(2))
ang = self.drone.theta
self.roll.append(ang.item(0))
self.pitch.append(ang.item(1))
self.yaw.append(ang.item(2))
#print self.theta_desired.item(2)
self.cmd1.append(inputCurrents[0] - inputCurrents[2])
self.cmd2.append(inputCurrents[1] - inputCurrents[3])
self.cmd3.append(inputCurrents[0] - inputCurrents[1])
self.cmd4.append(inputCurrents[3])
self.roll_des.append(self.theta_desired[0])
self.pitch_des.append(self.theta_desired[1])
self.yaw_des.append(self.theta_desired[2])
