def get_perturbed_trajectory_and_parms(self, strk_idx):
#registration points...
mdl = pytk.TrajKinMdl()
mdl.x0 = self.char_mdl[strk_idx]['start_pnt'][0]
mdl.y0 = self.char_mdl[strk_idx]['start_pnt'][1]
mdl.mdl_parms_ = self.char_mdl[strk_idx]['opt_parms']
vel_profile = self.char_mdl[strk_idx]['vel_profile']
reg_pnts_array = self.char_mdl[strk_idx]['reg_pnts_array']
t_array = np.arange(len(vel_profile))*self.dt
#opt
#get noise
num_parm_per_comp = 4
noise_ratio_array = []
for slider_lst in self.parms_sliders[strk_idx]:
for slider in slider_lst:
noise_ratio_array.append(float((slider.value()-50))/100)
noise_ratio_array = np.reshape(noise_ratio_array, (-1, num_parm_per_comp))
opt_parms = np.array(self.char_mdl[strk_idx]['opt_parms'])
for row in range(opt_parms.shape[0]):
opt_parms[row][0] += noise_ratio_array[row][0] * np.abs(opt_parms[row][0]) * 0.8
opt_parms[row][2] += noise_ratio_array[row][1] * np.abs(opt_parms[row][2]) * 0.5
opt_parms[row][3] += noise_ratio_array[row][2] * np.abs(opt_parms[row][3]) * 0.5
#theta_s & theta_e: noise is applied to delta_theta
opt_theta_s = opt_parms[row][4]
opt_theta_e = opt_parms[row][5]
opt_parms[row][4] = (opt_theta_s + opt_theta_e)/2 - (opt_theta_e-opt_theta_s) * (1 + noise_ratio_array[row][3]*2) / 2
opt_parms[row][5] = (opt_theta_s + opt_theta_e)/2 + (opt_theta_e-opt_theta_s) * (1 + noise_ratio_array[row][3]*2) / 2
traj_opt, vel_vec_opt = mdl.eval(t_array, opt_parms)
theta_opt = utils.get_continuous_ang(traj_opt)
return mdl, t_array, opt_parms, traj_opt, vel_vec_opt, theta_opt
def plot_data(self):
#plot data
#evaluate base
curr_data = [ mdl['stroke'] for mdl in self.char_mdl ]
bFirstStroke = True
last_stroke_end_t = 0.0
if 'vel_profile' not in self.char_mdl[0]:
print 'no velocity profile stored'
return
#currently, only consider one stroke
for strk_idx, stroke in enumerate(curr_data):
#vel profile
vel_profile = self.char_mdl[strk_idx]['vel_profile']
#t_array
t_array = np.linspace(0, 1.0, len(stroke)) + last_stroke_end_t
last_stroke_end_t = t_array[-1]
#vel vec & theta
vel_vec = np.diff(stroke, axis=0) / (t_array[1] - t_array[0])
#theta = np.arctan2(vel_vec[:, 1], vel_vec[:, 0])
theta = utils.get_continuous_ang(stroke)
#plot
#only data
#char profile
self.ax_char_prf.plot(stroke[:, 0], -stroke[:, 1], 'b', linewidth=4.0)
self.ax_char_prf.set_title('Character Profile', fontsize=8)
self.ax_char_prf.set_xlim([-1.5, 1.5])
self.ax_char_prf.set_ylim([-1.5, 1.5])
self.ax_char_prf.set_xticks([])
self.ax_char_prf.set_yticks([])
#vel_x & vel_y
self.ax_xvel.plot(t_array[0:-1], vel_vec[:, 0], 'b', linewidth=4.0)
self.ax_xvel.set_title('X Velocity', fontsize=8)
self.ax_xvel.set_xlabel('Time (s)', fontsize=8)
self.ax_xvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
self.ax_yvel.plot(t_array[0:-1], vel_vec[:, 1], 'b', linewidth=4.0)
self.ax_yvel.set_title('Y Velocity', fontsize=8)
self.ax_yvel.set_xlabel('Time (s)', fontsize=8)
self.ax_yvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
#vel profile
self.ax_vel_prf.plot(t_array, vel_profile, 'b', linewidth=4.0)
self.ax_vel_prf.set_title('Velocity Maganitude', fontsize=8)
self.ax_vel_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_vel_prf.set_ylabel('Maganitude (Unit/s)', fontsize=8)
#ang profile
self.ax_ang_prf.plot(t_array[0:-1], theta, 'b', linewidth=4.0)
self.ax_ang_prf.set_title('Angular Position', fontsize=8)
self.ax_ang_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_ang_prf.set_ylabel('Angular Position (rad)', fontsize=8)
if bFirstStroke:
self.ax_char_prf.hold(True)
self.ax_xvel.hold(True)
self.ax_yvel.hold(True)
self.ax_vel_prf.hold(True)
self.ax_ang_prf.hold(True)
bFirstStroke = False
colors = ['r', 'y', 'k', 'g', 'w']
last_stroke_end_t = 0.0
for curr_idx in range(len(self.char_mdl)):
#hold current drawings to add new curves
#now only the first stroke
#registration points...
vel_profile = self.char_mdl[curr_idx]['vel_profile']
t_array = np.linspace(0, 1.0, len(curr_data[curr_idx]))
if 'start_pnt' in self.char_mdl[curr_idx] and 'opt_parms' in self.char_mdl[curr_idx]:
x0 = self.char_mdl[curr_idx]['start_pnt'][0]
y0 = self.char_mdl[curr_idx]['start_pnt'][1]
opt_parms = np.array(self.char_mdl[curr_idx]['opt_parms'])
traj_opt, vel_vec_opt, opt_parms = self.traj_eval_helper(curr_idx, t_array, opt_parms, x0, y0)
theta_opt = utils.get_continuous_ang(traj_opt)
self.ax_char_prf.plot(traj_opt[:, 0], -traj_opt[:, 1], 'r', linewidth=4.0)
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(vel_vec_opt**2, axis=1)**(1./2), 'r', linewidth=4.0)
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 0], 'r', linewidth=4.0)
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 1], 'r', linewidth=4.0)
self.ax_ang_prf.plot(t_array[1:]+last_stroke_end_t, theta_opt, 'r', linewidth=4.0)
#for each component
for parm in opt_parms:
comp_traj, comp_vel_vec = pytkrxz.rxzero_traj_eval([parm], t_array, x0, y0)
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(comp_vel_vec**2, axis=1)**(1./2), 'g', linewidth=2.5)
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 0], 'g', linewidth=2.5)
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 1], 'g', linewidth=2.5)
last_stroke_end_t += t_array[-1]
else:
last_stroke_end_t += t_array[-1]
self.ax_char_prf.hold(False)
self.ax_xvel.hold(False)
self.ax_yvel.hold(False)
self.ax_vel_prf.hold(False)
self.ax_ang_prf.hold(False)
self.canvas.draw()
return
def plot_data(self):
#plot data
curr_data = self.get_current_data()
bFirstStroke = True
last_stroke_end_t = 0.0
#currently, only consider one stroke
for stroke in curr_data:
#vel profile
vel_profile = utils.get_vel_profile(stroke)/self.dt
#t_array
t_array = np.arange(len(vel_profile))*self.dt + last_stroke_end_t
last_stroke_end_t = t_array[-1]
#vel vec & theta
vel_vec = np.diff(stroke, axis=0)/self.dt
#theta = np.arctan2(vel_vec[:, 1], vel_vec[:, 0])
theta = utils.get_continuous_ang(stroke)
#plot
#only data
#char profile
self.ax_char_prf.plot(stroke[:, 0], -stroke[:, 1], 'b')
self.ax_char_prf.set_title('Character Profile', fontsize=8)
self.ax_char_prf.set_xticks([])
self.ax_char_prf.set_yticks([])
#vel_x & vel_y
self.ax_xvel.plot(t_array, vel_vec[:, 0], 'b')
self.ax_xvel.set_title('X Velocity', fontsize=8)
self.ax_xvel.set_xlabel('Time (s)', fontsize=8)
self.ax_xvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
self.ax_yvel.plot(t_array, vel_vec[:, 1], 'b')
self.ax_yvel.set_title('Y Velocity', fontsize=8)
self.ax_yvel.set_xlabel('Time (s)', fontsize=8)
self.ax_yvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
#vel profile
self.ax_vel_prf.plot(t_array, vel_profile, 'b')
self.ax_vel_prf.set_title('Velocity Maganitude', fontsize=8)
self.ax_vel_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_vel_prf.set_ylabel('Maganitude (Unit/s)', fontsize=8)
#ang profile
self.ax_ang_prf.plot(t_array, theta, 'b')
self.ax_ang_prf.set_title('Angular Position', fontsize=8)
self.ax_ang_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_ang_prf.set_ylabel('Angular Position (rad)', fontsize=8)
if bFirstStroke:
self.ax_char_prf.hold(True)
self.ax_xvel.hold(True)
self.ax_yvel.hold(True)
self.ax_vel_prf.hold(True)
self.ax_ang_prf.hold(True)
bFirstStroke = False
colors = ['r', 'y', 'k', 'g', 'w']
last_stroke_end_t = 0.0
for curr_idx in range(len(self.char_mdl)):
#hold current drawings to add new curves
mdl, t_array, opt_parms, traj_opt, vel_vec_opt, theta_opt = self.get_perturbed_trajectory_and_parms(curr_idx)
self.ax_char_prf.plot(traj_opt[:, 0], -traj_opt[:, 1], 'r')
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(vel_vec_opt**2, axis=1)**(1./2), 'r')
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 0], 'r')
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 1], 'r')
self.ax_ang_prf.plot(t_array[1:]+last_stroke_end_t, theta_opt, 'r')
#for each component
for parm in opt_parms:
comp_traj, comp_vel_vec = mdl.eval(t_array, [parm])
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(comp_vel_vec**2, axis=1)**(1./2), 'g')
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 0], 'g')
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 1], 'g')
last_stroke_end_t += t_array[-1]
self.ax_char_prf.hold(False)
self.ax_xvel.hold(False)
self.ax_yvel.hold(False)
self.ax_vel_prf.hold(False)
self.ax_ang_prf.hold(False)
self.canvas.draw()
return
def plot_data(self):
#plot data
curr_data = self.get_current_data()
bFirstStroke = True
last_stroke_end_t = 0.0
#currently, only consider one stroke
for stroke in curr_data:
#vel profile
vel_profile = utils.get_vel_profile(stroke) / self.dt
#t_array
t_array = np.arange(len(vel_profile)) * self.dt + last_stroke_end_t
last_stroke_end_t = t_array[-1]
#vel vec & theta
vel_vec = np.diff(stroke, axis=0) / self.dt
#theta = np.arctan2(vel_vec[:, 1], vel_vec[:, 0])
theta = utils.get_continuous_ang(stroke)
#plot
#only data
#char profile
self.ax_char_prf.plot(stroke[:, 0], -stroke[:, 1], 'b')
self.ax_char_prf.set_title('Character Profile', fontsize=8)
self.ax_char_prf.set_xticks([])
self.ax_char_prf.set_yticks([])
#vel_x & vel_y
self.ax_xvel.plot(t_array, vel_vec[:, 0], 'b')
self.ax_xvel.set_title('X Velocity', fontsize=8)
self.ax_xvel.set_xlabel('Time (s)', fontsize=8)
self.ax_xvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
self.ax_yvel.plot(t_array, vel_vec[:, 1], 'b')
self.ax_yvel.set_title('Y Velocity', fontsize=8)
self.ax_yvel.set_xlabel('Time (s)', fontsize=8)
self.ax_yvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
#vel profile
self.ax_vel_prf.plot(t_array, vel_profile, 'b')
self.ax_vel_prf.set_title('Velocity Maganitude', fontsize=8)
self.ax_vel_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_vel_prf.set_ylabel('Maganitude (Unit/s)', fontsize=8)
#ang profile
self.ax_ang_prf.plot(t_array, theta, 'b')
self.ax_ang_prf.set_title('Angular Position', fontsize=8)
self.ax_ang_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_ang_prf.set_ylabel('Angular Position (rad)', fontsize=8)
if bFirstStroke:
self.ax_char_prf.hold(True)
self.ax_xvel.hold(True)
self.ax_yvel.hold(True)
self.ax_vel_prf.hold(True)
self.ax_ang_prf.hold(True)
bFirstStroke = False
colors = ['r', 'y', 'k', 'g', 'w']
last_stroke_end_t = 0.0
for curr_idx in range(len(self.char_mdl)):
#hold current drawings to add new curves
#now only the first stroke
#registration points...
mdl = pytk.TrajKinMdl()
mdl.x0 = self.char_mdl[curr_idx]['start_pnt'][0]
mdl.y0 = self.char_mdl[curr_idx]['start_pnt'][1]
mdl.mdl_parms_ = self.char_mdl[curr_idx]['opt_parms']
vel_profile = self.char_mdl[curr_idx]['vel_profile']
reg_pnts_array = self.char_mdl[curr_idx]['reg_pnts_array']
t_array = np.arange(len(vel_profile)) * self.dt
#opt
#get noise
num_parm_per_comp = 4
noise_ratio_array = []
for slider_lst in self.parms_sliders[curr_idx]:
for slider in slider_lst:
noise_ratio_array.append(
float((slider.value() - 50)) / 100)
noise_ratio_array = np.reshape(noise_ratio_array,
(-1, num_parm_per_comp))
opt_parms = np.array(self.char_mdl[curr_idx]['opt_parms'])
for row in range(opt_parms.shape[0]):
opt_parms[row][0] += noise_ratio_array[row][0] * np.abs(
opt_parms[row][0]) * 0.8
opt_parms[row][2] += noise_ratio_array[row][1] * np.abs(
opt_parms[row][2]) * 0.5
opt_parms[row][3] += noise_ratio_array[row][2] * np.abs(
opt_parms[row][3]) * 0.5
#theta_s & theta_e: noise is applied to delta_theta
opt_theta_s = opt_parms[row][4]
opt_theta_e = opt_parms[row][5]
opt_parms[row][4] = (opt_theta_s + opt_theta_e) / 2 - (
opt_theta_e -
opt_theta_s) * (1 + noise_ratio_array[row][3] * 2) / 2
opt_parms[row][5] = (opt_theta_s + opt_theta_e) / 2 + (
opt_theta_e -
opt_theta_s) * (1 + noise_ratio_array[row][3] * 2) / 2
traj_opt, vel_vec_opt = mdl.eval(t_array, opt_parms)
theta_opt = utils.get_continuous_ang(traj_opt)
self.ax_char_prf.plot(traj_opt[:, 0], -traj_opt[:, 1], 'r')
self.ax_vel_prf.plot(t_array[:] + last_stroke_end_t,
np.sum(vel_vec_opt**2, axis=1)**(1. / 2), 'r')
self.ax_xvel.plot(t_array[:] + last_stroke_end_t,
vel_vec_opt[:, 0], 'r')
self.ax_yvel.plot(t_array[:] + last_stroke_end_t,
vel_vec_opt[:, 1], 'r')
self.ax_ang_prf.plot(t_array[1:] + last_stroke_end_t, theta_opt,
'r')
#for each component
for parm in opt_parms:
comp_traj, comp_vel_vec = mdl.eval(t_array, [parm])
self.ax_vel_prf.plot(t_array[:] + last_stroke_end_t,
np.sum(comp_vel_vec**2, axis=1)**(1. / 2),
'g')
self.ax_xvel.plot(t_array[:] + last_stroke_end_t,
comp_vel_vec[:, 0], 'g')
self.ax_yvel.plot(t_array[:] + last_stroke_end_t,
comp_vel_vec[:, 1], 'g')
last_stroke_end_t += t_array[-1]
self.ax_char_prf.hold(False)
self.ax_xvel.hold(False)
self.ax_yvel.hold(False)
self.ax_vel_prf.hold(False)
self.ax_ang_prf.hold(False)
self.canvas.draw()
return
def plot_data(self):
#plot data
#evaluate base
curr_data = [mdl['stroke'] for mdl in self.char_mdl]
bFirstStroke = True
last_stroke_end_t = 0.0
if 'vel_profile' not in self.char_mdl[0]:
print 'no velocity profile stored'
return
#currently, only consider one stroke
for strk_idx, stroke in enumerate(curr_data):
#vel profile
vel_profile = self.char_mdl[strk_idx]['vel_profile']
#t_array
t_array = np.linspace(0, 1.0, len(stroke)) + last_stroke_end_t
last_stroke_end_t = t_array[-1]
#vel vec & theta
vel_vec = np.diff(stroke, axis=0) / (t_array[1] - t_array[0])
#theta = np.arctan2(vel_vec[:, 1], vel_vec[:, 0])
theta = utils.get_continuous_ang(stroke)
#plot
#only data
#char profile
self.ax_char_prf.plot(stroke[:, 0],
-stroke[:, 1],
'b',
linewidth=4.0)
self.ax_char_prf.set_title('Character Profile', fontsize=8)
self.ax_char_prf.set_xlim([-1.5, 1.5])
self.ax_char_prf.set_ylim([-1.5, 1.5])
self.ax_char_prf.set_xticks([])
self.ax_char_prf.set_yticks([])
#vel_x & vel_y
self.ax_xvel.plot(t_array[0:-1], vel_vec[:, 0], 'b', linewidth=4.0)
self.ax_xvel.set_title('X Velocity', fontsize=8)
self.ax_xvel.set_xlabel('Time (s)', fontsize=8)
self.ax_xvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
self.ax_yvel.plot(t_array[0:-1], vel_vec[:, 1], 'b', linewidth=4.0)
self.ax_yvel.set_title('Y Velocity', fontsize=8)
self.ax_yvel.set_xlabel('Time (s)', fontsize=8)
self.ax_yvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
#vel profile
self.ax_vel_prf.plot(t_array, vel_profile, 'b', linewidth=4.0)
self.ax_vel_prf.set_title('Velocity Maganitude', fontsize=8)
self.ax_vel_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_vel_prf.set_ylabel('Maganitude (Unit/s)', fontsize=8)
#ang profile
self.ax_ang_prf.plot(t_array[0:-1], theta, 'b', linewidth=4.0)
self.ax_ang_prf.set_title('Angular Position', fontsize=8)
self.ax_ang_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_ang_prf.set_ylabel('Angular Position (rad)', fontsize=8)
if bFirstStroke:
self.ax_char_prf.hold(True)
self.ax_xvel.hold(True)
self.ax_yvel.hold(True)
self.ax_vel_prf.hold(True)
self.ax_ang_prf.hold(True)
bFirstStroke = False
colors = ['r', 'y', 'k', 'g', 'w']
last_stroke_end_t = 0.0
for curr_idx in range(len(self.char_mdl)):
#hold current drawings to add new curves
#now only the first stroke
#registration points...
vel_profile = self.char_mdl[curr_idx]['vel_profile']
t_array = np.linspace(0, 1.0, len(curr_data[curr_idx]))
if 'start_pnt' in self.char_mdl[
curr_idx] and 'opt_parms' in self.char_mdl[curr_idx]:
x0 = self.char_mdl[curr_idx]['start_pnt'][0]
y0 = self.char_mdl[curr_idx]['start_pnt'][1]
opt_parms = np.array(self.char_mdl[curr_idx]['opt_parms'])
traj_opt, vel_vec_opt, opt_parms = self.traj_eval_helper(
curr_idx, t_array, opt_parms, x0, y0)
theta_opt = utils.get_continuous_ang(traj_opt)
self.ax_char_prf.plot(traj_opt[:, 0],
-traj_opt[:, 1],
'r',
linewidth=4.0)
self.ax_vel_prf.plot(t_array[:] + last_stroke_end_t,
np.sum(vel_vec_opt**2, axis=1)**(1. / 2),
'r',
linewidth=4.0)
self.ax_xvel.plot(t_array[:] + last_stroke_end_t,
vel_vec_opt[:, 0],
'r',
linewidth=4.0)
self.ax_yvel.plot(t_array[:] + last_stroke_end_t,
vel_vec_opt[:, 1],
'r',
linewidth=4.0)
self.ax_ang_prf.plot(t_array[1:] + last_stroke_end_t,
theta_opt,
'r',
linewidth=4.0)
#for each component
for parm in opt_parms:
comp_traj, comp_vel_vec = pytkrxz.rxzero_traj_eval([parm],
t_array,
x0, y0)
self.ax_vel_prf.plot(t_array[:] + last_stroke_end_t,
np.sum(comp_vel_vec**2,
axis=1)**(1. / 2),
'g',
linewidth=2.5)
self.ax_xvel.plot(t_array[:] + last_stroke_end_t,
comp_vel_vec[:, 0],
'g',
linewidth=2.5)
self.ax_yvel.plot(t_array[:] + last_stroke_end_t,
comp_vel_vec[:, 1],
'g',
linewidth=2.5)
last_stroke_end_t += t_array[-1]
else:
last_stroke_end_t += t_array[-1]
self.ax_char_prf.hold(False)
self.ax_xvel.hold(False)
self.ax_yvel.hold(False)
self.ax_vel_prf.hold(False)
self.ax_ang_prf.hold(False)
self.canvas.draw()
return
def plot_data(self):
#plot data
curr_data = self.get_current_data()
bFirstStroke = True
last_stroke_end_t = 0.0
#currently, only consider one stroke
for stroke in curr_data:
#vel profile
vel_profile = utils.get_vel_profile(stroke)/self.dt
#t_array
t_array = np.arange(len(vel_profile))*self.dt + last_stroke_end_t
last_stroke_end_t = t_array[-1]
#vel vec & theta
vel_vec = np.diff(stroke, axis=0)/self.dt
#theta = np.arctan2(vel_vec[:, 1], vel_vec[:, 0])
theta = utils.get_continuous_ang(stroke)
#plot
#only data
#char profile
self.ax_char_prf.plot(stroke[:, 0], -stroke[:, 1], 'b')
self.ax_char_prf.set_title('Character Profile', fontsize=8)
self.ax_char_prf.set_xticks([])
self.ax_char_prf.set_yticks([])
#vel_x & vel_y
self.ax_xvel.plot(t_array, vel_vec[:, 0], 'b')
self.ax_xvel.set_title('X Velocity', fontsize=8)
self.ax_xvel.set_xlabel('Time (s)', fontsize=8)
self.ax_xvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
self.ax_yvel.plot(t_array, vel_vec[:, 1], 'b')
self.ax_yvel.set_title('Y Velocity', fontsize=8)
self.ax_yvel.set_xlabel('Time (s)', fontsize=8)
self.ax_yvel.set_ylabel('Velocity (Unit/s)', fontsize=8)
#vel profile
self.ax_vel_prf.plot(t_array, vel_profile, 'b')
self.ax_vel_prf.set_title('Velocity Maganitude', fontsize=8)
self.ax_vel_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_vel_prf.set_ylabel('Maganitude (Unit/s)', fontsize=8)
#ang profile
self.ax_ang_prf.plot(t_array, theta, 'b')
self.ax_ang_prf.set_title('Angular Position', fontsize=8)
self.ax_ang_prf.set_xlabel('Time (s)', fontsize=8)
self.ax_ang_prf.set_ylabel('Angular Position (rad)', fontsize=8)
if bFirstStroke:
self.ax_char_prf.hold(True)
self.ax_xvel.hold(True)
self.ax_yvel.hold(True)
self.ax_vel_prf.hold(True)
self.ax_ang_prf.hold(True)
bFirstStroke = False
colors = ['r', 'y', 'k', 'g', 'w']
last_stroke_end_t = 0.0
for curr_idx in range(len(self.char_mdl)):
#hold current drawings to add new curves
#now only the first stroke
#registration points...
mdl = pytk.TrajKinMdl()
mdl.x0 = self.char_mdl[curr_idx]['start_pnt'][0]
mdl.y0 = self.char_mdl[curr_idx]['start_pnt'][1]
mdl.mdl_parms_ = self.char_mdl[curr_idx]['opt_parms']
vel_profile = self.char_mdl[curr_idx]['vel_profile']
reg_pnts_array = self.char_mdl[curr_idx]['reg_pnts_array']
t_array = np.arange(len(vel_profile))*self.dt
#opt
#get noise
num_parm_per_comp = 4
noise_ratio_array = []
for slider_lst in self.parms_sliders[curr_idx]:
for slider in slider_lst:
noise_ratio_array.append(float((slider.value()-50))/100)
noise_ratio_array = np.reshape(noise_ratio_array, (-1, num_parm_per_comp))
opt_parms = np.array(self.char_mdl[curr_idx]['opt_parms'])
for row in range(opt_parms.shape[0]):
opt_parms[row][0] += noise_ratio_array[row][0] * np.abs(opt_parms[row][0]) * 0.8
opt_parms[row][2] += noise_ratio_array[row][1] * np.abs(opt_parms[row][2]) * 0.5
opt_parms[row][3] += noise_ratio_array[row][2] * np.abs(opt_parms[row][3]) * 0.5
#theta_s & theta_e: noise is applied to delta_theta
opt_theta_s = opt_parms[row][4]
opt_theta_e = opt_parms[row][5]
opt_parms[row][4] = (opt_theta_s + opt_theta_e)/2 - (opt_theta_e-opt_theta_s) * (1 + noise_ratio_array[row][3]*2) / 2
opt_parms[row][5] = (opt_theta_s + opt_theta_e)/2 + (opt_theta_e-opt_theta_s) * (1 + noise_ratio_array[row][3]*2) / 2
traj_opt, vel_vec_opt = mdl.eval(t_array, opt_parms)
theta_opt = utils.get_continuous_ang(traj_opt)
self.ax_char_prf.plot(traj_opt[:, 0], -traj_opt[:, 1], 'r')
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(vel_vec_opt**2, axis=1)**(1./2), 'r')
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 0], 'r')
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, vel_vec_opt[:, 1], 'r')
self.ax_ang_prf.plot(t_array[1:]+last_stroke_end_t, theta_opt, 'r')
#for each component
for parm in opt_parms:
comp_traj, comp_vel_vec = mdl.eval(t_array, [parm])
self.ax_vel_prf.plot(t_array[:]+last_stroke_end_t, np.sum(comp_vel_vec**2, axis=1)**(1./2), 'g')
self.ax_xvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 0], 'g')
self.ax_yvel.plot(t_array[:]+last_stroke_end_t, comp_vel_vec[:, 1], 'g')
last_stroke_end_t += t_array[-1]
self.ax_char_prf.hold(False)
self.ax_xvel.hold(False)
self.ax_yvel.hold(False)
self.ax_vel_prf.hold(False)
self.ax_ang_prf.hold(False)
self.canvas.draw()
return
