def compute_state_space_trajectory_and_derivatives(p,psi,dt): num_timesteps = len(p) psi = trigutils.compute_continuous_angle_array(psi) p_dot = c_[ gradient(p[:,0],dt), gradient(p[:,1],dt), gradient(p[:,2],dt)] p_dot_dot = c_[ gradient(p_dot[:,0],dt), gradient(p_dot[:,1],dt), gradient(p_dot[:,2],dt)] f_thrust_world = m*p_dot_dot - f_external_world.T.A f_thrust_world_normalized = sklearn.preprocessing.normalize(f_thrust_world) z_axis_intermediate = c_[ cos(psi), zeros_like(psi), -sin(psi) ] y_axis = f_thrust_world_normalized x_axis = sklearn.preprocessing.normalize(cross(z_axis_intermediate, y_axis)) z_axis = sklearn.preprocessing.normalize(cross(y_axis, x_axis)) theta = zeros(num_timesteps) psi_recomputed = zeros(num_timesteps) phi = zeros(num_timesteps) for ti in range(num_timesteps): z_axis_ti = c_[matrix(z_axis[ti]),0].T y_axis_ti = c_[matrix(y_axis[ti]),0].T x_axis_ti = c_[matrix(x_axis[ti]),0].T R_world_from_body_ti = c_[z_axis_ti,y_axis_ti,x_axis_ti,[0,0,0,1]] psi_recomputed_ti,theta_ti,phi_ti = transformations.euler_from_matrix(R_world_from_body_ti,"ryxz") assert allclose(R_world_from_body_ti, transformations.euler_matrix(psi_recomputed_ti,theta_ti,phi_ti,"ryxz")) theta[ti] = theta_ti psi_recomputed[ti] = psi_recomputed_ti phi[ti] = phi_ti theta = trigutils.compute_continuous_angle_array(theta) psi_recomputed = trigutils.compute_continuous_angle_array(psi_recomputed) phi = trigutils.compute_continuous_angle_array(phi) assert allclose(psi_recomputed, psi) psi = psi_recomputed theta_dot = gradient(theta,dt) psi_dot = gradient(psi,dt) phi_dot = gradient(phi,dt) theta_dot_dot = gradient(theta_dot,dt) psi_dot_dot = gradient(psi_dot,dt) phi_dot_dot = gradient(phi_dot,dt) return p, p_dot, p_dot_dot, theta, theta_dot, theta_dot_dot, psi, psi_dot, psi_dot_dot, phi, phi_dot, phi_dot_dot
def compute_state_space_trajectory_and_derivatives(p, dt): f_thrust_world_norm_threshold = 0.01 num_timesteps = len(p) p_dot = c_[gradient(p[:, 0], dt), gradient(p[:, 1], dt)] p_dot_dot = c_[gradient(p_dot[:, 0], dt), gradient(p_dot[:, 1], dt)] f_thrust_world = m * p_dot_dot - f_external_world.T.A f_thrust_world_norm = linalg.norm(f_thrust_world, axis=1) theta_raw = arctan2(f_thrust_world[:, 0], f_thrust_world[:, 1]) - (pi / 2.0) t_tmp = arange(num_timesteps) theta_func = scipy.interpolate.interp1d( t_tmp[f_thrust_world_norm > f_thrust_world_norm_threshold], theta_raw[f_thrust_world_norm > f_thrust_world_norm_threshold], kind="linear") theta = theta_func(t_tmp) theta = trigutils.compute_continuous_angle_array(theta) theta_dot = gradient(theta, dt) theta_dot_dot = gradient(theta_dot, dt) return p, p_dot, p_dot_dot, theta, theta_dot, theta_dot_dot
def compute_state_space_trajectory_and_derivatives(p_body,p_look_at,y_axis_cam_hint,dt): num_timesteps = len(p_body) # # compute the yaw, roll, and pitch of the quad using differential flatness # p_body_dot = c_[ gradient(p_body[:,0],dt), gradient(p_body[:,1],dt), gradient(p_body[:,2],dt)] p_body_dot_dot = c_[ gradient(p_body_dot[:,0],dt), gradient(p_body_dot[:,1],dt), gradient(p_body_dot[:,2],dt)] f_thrust_world = m*p_body_dot_dot - f_external_world.T.A f_thrust_world_normalized = sklearn.preprocessing.normalize(f_thrust_world) y_axis_body = f_thrust_world_normalized z_axis_body = sklearn.preprocessing.normalize(cross(y_axis_body, p_look_at - p_body)) x_axis_body = sklearn.preprocessing.normalize(cross(z_axis_body, y_axis_body)) R_world_from_body = zeros((num_timesteps,4,4)) theta_body = zeros(num_timesteps) psi_body = zeros(num_timesteps) phi_body = zeros(num_timesteps) for ti in range(num_timesteps): z_axis_body_ti = c_[matrix(z_axis_body[ti]),0].T y_axis_body_ti = c_[matrix(y_axis_body[ti]),0].T x_axis_body_ti = c_[matrix(x_axis_body[ti]),0].T R_world_from_body_ti = c_[z_axis_body_ti,y_axis_body_ti,x_axis_body_ti,[0,0,0,1]] psi_body_ti,theta_body_ti,phi_body_ti = transformations.euler_from_matrix(R_world_from_body_ti,"ryxz") assert allclose(R_world_from_body_ti, transformations.euler_matrix(psi_body_ti,theta_body_ti,phi_body_ti,"ryxz")) R_world_from_body[ti] = R_world_from_body_ti theta_body[ti] = theta_body_ti psi_body[ti] = psi_body_ti phi_body[ti] = phi_body_ti psi_body = trigutils.compute_continuous_angle_array(psi_body) theta_body = trigutils.compute_continuous_angle_array(theta_body) phi_body = trigutils.compute_continuous_angle_array(phi_body) # # now that we have the full orientation of the quad, compute the full orientation of the camera relative to the quad # x_axis_cam = sklearn.preprocessing.normalize( p_look_at - p_body ) z_axis_cam = sklearn.preprocessing.normalize( cross(y_axis_cam_hint, x_axis_cam) ) y_axis_cam = sklearn.preprocessing.normalize( cross(x_axis_cam, z_axis_cam) ) theta_cam = zeros(num_timesteps) psi_cam = zeros(num_timesteps) phi_cam = zeros(num_timesteps) for ti in range(num_timesteps): z_axis_cam_ti = c_[matrix(z_axis_cam[ti]),0].T y_axis_cam_ti = c_[matrix(y_axis_cam[ti]),0].T x_axis_cam_ti = c_[matrix(x_axis_cam[ti]),0].T R_world_from_cam_ti = matrix(c_[z_axis_cam_ti, y_axis_cam_ti, x_axis_cam_ti, [0,0,0,1]]) R_world_from_body_ti = matrix(R_world_from_body[ti]) R_body_from_cam_ti = R_world_from_body_ti.T*R_world_from_cam_ti psi_cam_ti, theta_cam_ti, phi_cam_ti = transformations.euler_from_matrix(R_body_from_cam_ti,"ryxz") # # sanity check that world-from-body rotation matrix we compute actually # maps the vector [0,0,1] to the quadrotor x axis # assert allclose(c_[matrix(x_axis_body[ti]),1].T, R_world_from_body_ti*matrix([0,0,1,1]).T) # # sanity check that the world-from-camera rotation matrix we compute actually # maps the vector [0,0,1] to the camera x axis # assert allclose(c_[matrix(x_axis_cam[ti]),1].T, R_world_from_cam_ti*matrix([0,0,1,1]).T) # # sanity check that the world-from-body and body-from-camera rotation matrices # we compute actually maps the vector [0,0,1] to the camera x axis # assert allclose(c_[matrix(x_axis_cam[ti]),1].T, R_world_from_body_ti*R_body_from_cam_ti*matrix([0,0,1,1]).T) theta_cam[ti] = theta_cam_ti psi_cam[ti] = psi_cam_ti phi_cam[ti] = phi_cam_ti theta_cam = trigutils.compute_continuous_angle_array(theta_cam) psi_cam = trigutils.compute_continuous_angle_array(psi_cam) phi_cam = trigutils.compute_continuous_angle_array(phi_cam) # # assert that we never need any camera yaw in the body frame of the quad # assert allclose(psi_cam, 0) # # compute derivatives # theta_body_dot = gradient(theta_body,dt) psi_body_dot = gradient(psi_body,dt) phi_body_dot = gradient(phi_body,dt) theta_cam_dot = gradient(theta_cam,dt) psi_cam_dot = gradient(psi_cam,dt) phi_cam_dot = gradient(phi_cam,dt) theta_body_dot_dot = gradient(theta_body_dot,dt) psi_body_dot_dot = gradient(psi_body_dot,dt) phi_body_dot_dot = gradient(phi_body_dot,dt) theta_cam_dot_dot = gradient(theta_cam_dot,dt) psi_cam_dot_dot = gradient(psi_cam_dot,dt) phi_cam_dot_dot = gradient(phi_cam_dot,dt) return p_body, p_body_dot, p_body_dot_dot, theta_body, theta_body_dot, theta_body_dot_dot, psi_body, psi_body_dot, psi_body_dot_dot, phi_body, phi_body_dot, phi_body_dot_dot, theta_cam, theta_cam_dot, theta_cam_dot_dot, psi_cam, psi_cam_dot, psi_cam_dot_dot, phi_cam, phi_cam_dot, phi_cam_dot_dot
def compute_state_space_trajectory_and_derivatives(p, psi, dt): num_timesteps = len(p) psi = trigutils.compute_continuous_angle_array(psi) p_dot = c_[gradient(p[:, 0], dt), gradient(p[:, 1], dt), gradient(p[:, 2], dt)] p_dot_dot = c_[gradient(p_dot[:, 0], dt), gradient(p_dot[:, 1], dt), gradient(p_dot[:, 2], dt)] f_thrust_world = m * p_dot_dot - f_external_world.T.A f_thrust_world_normalized = sklearn.preprocessing.normalize(f_thrust_world) z_axis_intermediate = c_[cos(psi), zeros_like(psi), -sin(psi)] y_axis = f_thrust_world_normalized x_axis = sklearn.preprocessing.normalize(cross(z_axis_intermediate, y_axis)) z_axis = sklearn.preprocessing.normalize(cross(y_axis, x_axis)) theta = zeros(num_timesteps) psi_recomputed = zeros(num_timesteps) phi = zeros(num_timesteps) for ti in range(num_timesteps): z_axis_ti = c_[matrix(z_axis[ti]), 0].T y_axis_ti = c_[matrix(y_axis[ti]), 0].T x_axis_ti = c_[matrix(x_axis[ti]), 0].T R_world_from_body_ti = c_[z_axis_ti, y_axis_ti, x_axis_ti, [0, 0, 0, 1]] psi_recomputed_ti, theta_ti, phi_ti = transformations.euler_from_matrix( R_world_from_body_ti, "ryxz") assert allclose( R_world_from_body_ti, transformations.euler_matrix(psi_recomputed_ti, theta_ti, phi_ti, "ryxz")) theta[ti] = theta_ti psi_recomputed[ti] = psi_recomputed_ti phi[ti] = phi_ti theta = trigutils.compute_continuous_angle_array(theta) psi_recomputed = trigutils.compute_continuous_angle_array(psi_recomputed) phi = trigutils.compute_continuous_angle_array(phi) assert allclose(psi_recomputed, psi) psi = psi_recomputed theta_dot = gradient(theta, dt) psi_dot = gradient(psi, dt) phi_dot = gradient(phi, dt) theta_dot_dot = gradient(theta_dot, dt) psi_dot_dot = gradient(psi_dot, dt) phi_dot_dot = gradient(phi_dot, dt) return p, p_dot, p_dot_dot, theta, theta_dot, theta_dot_dot, psi, psi_dot, psi_dot_dot, phi, phi_dot, phi_dot_dot