2022 IB Diploma Extended Essays

dx[-1] = (x[-1] - x[-2]) / (t[-1] - t[-2]) wz = dx

t = t_pose x = x_pose

dx = np.zeros(x.shape, np.float) dx[0:-1] = np.diff(x) / np.diff(t) dx[-1] = (x[-1] - x[-2]) / (t[-1] - t[-2]) vx = dx ddx = np.zeros(dx.shape, np.float) ddx[0:-1] = np.diff(dx) / np.diff(t) ddx[-1] = (dx[-1] - dx[-2]) / (t[-1] - t[-2]) dy = np.zeros(y.shape, np.float) dy[0:-1] = np.diff(y) / np.diff(t) dy[-1] = (y[-1] - y[-2]) / (t[-1] - t[-2]) vy = dy ddy = np.zeros(dy.shape, np.float) ddy[0:-1] = np.diff(dy) / np.diff(t) ddy[-1] = (dy[-1] - dy[-2]) / (t[-1] - t[-2]) vx_new = np.zeros(vx.shape, np.float) vy_new = np.zeros(vy.shape, np.float) for i in range(len(vx)-1): t = t_pose y = y_pose

vx_new[i+1] = cos(yaw_pose[i]) * vx[i+1] + sin(yaw_pose[i]) * vy[i+1] vy_new[i+1] = sin(yaw_pose[i]) * (-1)* vx[i+1] + cos(yaw_pose[i])* vy[i+1]

f_vx = open(path+'vx_robot_frame_lidar.txt', 'w') f_vy = open(path+'vy_robot_frame_lidar.txt', 'w') f_wz = open(path+'wz_robot_frame_lidar.txt', 'w')

printToFile(f_vx,t_pose,vx_new) printToFile(f_vy,t_pose,vy_new) printToFile(f_wz,t_pose,wz)

f_plot(t_pose, vx_new, colors=colors, linewidth=2.) f_plot(t_pose, vy_new, colors=colors, linewidth=2.) f_plot(t_pose, wz, colors=colors, linewidth=2.) f_plot(t_pose, dx, colors=colors, linewidth=2.) f_plot(t_pose, dy, colors=colors, linewidth=2.) ''' f_plot(t_pose, x_pose, colors=colors, linewidth=2.) f_plot(t_pose, y_pose, colors=colors, linewidth=2.) f_plot(t_pose, ddx, colors=colors, linewidth=2.) f_plot(t_pose, ddy, colors=colors, linewidth=2.) f_plot(x_pose, y_pose, colors=colors, linewidth=2.) '''