Python find_omega_theta Examples

Example #1

File: ar_tag_subs.py Project: bertheletc/save-the-zumy

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """

    R = rbt[0:3][:, 0:3]
    trans = rbt[0:3][:, 3:4]

    [omega, theta] = eqf.find_omega_theta(R)

    v = eqf.find_v(omega, theta, trans)

    #pdb.set_trace()

    v = np.array([v[0, 0], v[1, 0], v[2, 0]])

    twist = np.array([v[0], v[1], v[2], omega[0], omega[1], omega[2]])

    return twist

Example #2

File: kalman_ar_track.py Project: dHutchings/ee106a

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    #YOUR CODE HERE

    rot_matrix = rbt[0:3, 0:3]
    trans = rbt[0:3, 3]

    (omega, theta) = eqf.find_omega_theta(rot_matrix)
    v = eqf.find_v(omega, theta, trans)

    v = v.flatten()

    #v = np.array(3,1)
    #w = np.array(3,1)
    #print(v)
    #print(omega)
    return (v, omega)

Example #3

File: ar_tag_subs.py Project: dHutchings/ee106a

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    #YOUR CODE HERE
    
    rot_matrix = rbt[0:3,0:3]
    trans = rbt[0:3,3]
    
    (omega,theta) = eqf.find_omega_theta(rot_matrix)
    v = eqf.find_v(omega,theta,trans)

    v = v.flatten()

    #v = np.array(3,1)
    #w = np.array(3,1)
    #print(v)
    #print(omega)
    return (v,omega)

Example #4

File: follow_ar_tag_manual.py Project: kangnari/Utensil-Sort

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    #YOUR CODE HERE
    omega, theta = eqf.find_omega_theta(rbt)
    v = eqf.find_v(omega, theta, rbt[:3, 3])
    return (v,omega)

Example #5

File: follow_ar_tag_twist.py Project: bigdayangyu/ROS-project

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    #YOUR CODE HERE
    R = rbt[:3, :3]
    trans = rbt[:3, 3]
    orientation = eqf.find_omega_theta(R)  # omega/theta
    v = eqf.find_v(orientation[0], orientation[1], trans).reshape(3, )
    return (v, orientation[0])

Example #6

File: ar_tag_subs.py Project: areardon11/RoboCup

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    
    r = rbt[:3,:3]
    omega, theta = eqf.find_omega_theta(r)
    trans = rbt[0:3,3]

    v = eqf.find_v(omega, theta, trans)
    w = omega
    return (v,w)

Example #7

File: ar_tag_subs.py Project: limabeans/Baxter-the-Bartender

def compute_twist(rbt):
    """
    Computes the corresponding twist for the rigid body transform

    Input:
        rbt - (4,4) ndarray 

    Output:
        v - (3,) array
        w - (3,) array
    """
    #YOUR CODE HERE
    rbt = list(rbt)
    print 'here'
    R = np.array([[rbt[0][0], rbt[0][1], rbt[0][2]],
                  [rbt[1][0], rbt[1][1], rbt[1][2]],
                  [rbt[2][0], rbt[2][1], rbt[2][2]]])
    # p = np.array([[rbt[0][3]], [rbt[1][3]], [rbt[2][3]]])
    p = np.array([rbt[0][3], rbt[1][3], rbt[2][3]])
    omega, theta = eqf.find_omega_theta(R)
    v = eqf.find_v(omega, theta, p)
    return (v, omega)