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
dataset/ETHPy150Open stanford-gfx/Horus/Code/flashlight/quadrotor2d.py/compute_state_space_trajectory_and_derivatives/original