Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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