Python quaternion_matrix Examples

Example #1

File: ros_utils.py Project: cameronlee/python

    def draw_trajectory(self, pose_array, angles, ns="default_ns"):
        decimation = max(len(pose_array.poses)//6, 1)
        ps = gm.PoseStamped()
        ps.header.frame_id = pose_array.header.frame_id        
        ps.header.stamp = rospy.Time.now()
        handles = []
 
        multiplier = 5.79 
        
        for (pose,angle) in zip(pose_array.poses,angles)[::decimation]:

            ps.pose = deepcopy(pose)
            pointing_axis = transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])[:3,0]
            ps.pose.position.x -= .18*pointing_axis[0]
            ps.pose.position.y -= .18*pointing_axis[1]
            ps.pose.position.z -= .18*pointing_axis[2]
            
            
            root_link = "r_wrist_roll_link"
            valuedict = {"r_gripper_l_finger_joint":angle*multiplier,
                         "r_gripper_r_finger_joint":angle*multiplier,
                         "r_gripper_l_finger_tip_joint":angle*multiplier,
                         "r_gripper_r_finger_tip_joint":angle*multiplier,
                         "r_gripper_joint":angle}
            handles.extend(self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
        return handles

Example #2

File: ros_utils.py Project: warriorarmentaix/lfd

    def draw_trajectory(self, pose_array, angles, ns="default_ns"):
        decimation = max(len(pose_array.poses) // 6, 1)
        ps = gm.PoseStamped()
        ps.header.frame_id = pose_array.header.frame_id
        ps.header.stamp = rospy.Time.now()
        handles = []

        multiplier = 5.79

        for (pose, angle) in zip(pose_array.poses, angles)[::decimation]:

            ps.pose = deepcopy(pose)
            pointing_axis = transformations.quaternion_matrix([
                pose.orientation.x, pose.orientation.y, pose.orientation.z,
                pose.orientation.w
            ])[:3, 0]
            ps.pose.position.x -= .18 * pointing_axis[0]
            ps.pose.position.y -= .18 * pointing_axis[1]
            ps.pose.position.z -= .18 * pointing_axis[2]

            root_link = "r_wrist_roll_link"
            valuedict = {
                "r_gripper_l_finger_joint": angle * multiplier,
                "r_gripper_r_finger_joint": angle * multiplier,
                "r_gripper_l_finger_tip_joint": angle * multiplier,
                "r_gripper_r_finger_tip_joint": angle * multiplier,
                "r_gripper_joint": angle
            }
            handles.extend(
                self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
        return handles

Example #3

File: conversions.py Project: warriorarmentaix/lfd

def trans_rot_to_hmat(trans, rot):
    ''' 
    Converts a rotation and translation to a homogeneous transform. 

    **Args:** 

        **trans (np.array):** Translation (x, y, z). 

        **rot (np.array):** Quaternion (x, y, z, w). 

    **Returns:** 
        H (np.array): 4x4 homogenous transform matrix. 
    '''
    H = transformations.quaternion_matrix(rot)
    H[0:3, 3] = trans
    return H

Example #4

File: conversions.py Project: hojonathanho/python

def trans_rot_to_hmat(trans, rot): 
    ''' 
    Converts a rotation and translation to a homogeneous transform. 

    **Args:** 

        **trans (np.array):** Translation (x, y, z). 

        **rot (np.array):** Quaternion (x, y, z, w). 

    **Returns:** 
        H (np.array): 4x4 homogenous transform matrix. 
    ''' 
    H = transformations.quaternion_matrix(rot) 
    H[0:3, 3] = trans 
    return H

Example #5

File: lookup_and_execute_rope2.py Project: ankush-me/python

def quatswap(quat):
    mat = transformations.quaternion_matrix(quat)
    mat_new = np.c_[mat[:,2],mat[:,1],-mat[:,0],mat[:,3]]
    return transformations.quaternion_from_matrix(mat_new)

Example #6

def quatswap(quat):
    mat = transformations.quaternion_matrix(quat)
    mat_new = np.c_[mat[:, 2], mat[:, 1], -mat[:, 0], mat[:, 3]]
    return transformations.quaternion_from_matrix(mat_new)

Example #7

def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1)
    quat2_mat = jut.quaternion_matrix(quat2)
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return euclidean_dist(diff_quat, [0, 0, 0, 1])

Example #8

File: multi_item_translation_test.py Project: cameronlee/python

def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1) 
    quat2_mat = jut.quaternion_matrix(quat2) 
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return euclidean_dist(diff_quat, [0, 0, 0, 1])

Example #9

File: test_reachability_bimanual.py Project: pombredanne/python-10

handles = []
handles.append(reach.ENV.drawlinestrip(points=base_xyzs, linewidth=3.0))

handles.append(reach.ENV.drawlinestrip(points=states["xyz_r"], linewidth=3.0))
handles.append(reach.ENV.drawlinestrip(points=states["xyz_l"], linewidth=3.0, colors=(0, 0, 1)))


for i in xrange(len(base_xys)):
    base_tf = np.eye(4)
    base_tf[0:2, 3] = base_xys[i]

    xlarm, ylarm, zlarm = states["xyz_l"][i]
    xrarm, yrarm, zrarm = states["xyz_r"][i]

    larm_tf = tf.quaternion_matrix(states["quat_l"][i])
    rarm_tf = tf.quaternion_matrix(states["quat_r"][i])
    larm_tf[:3, 3] += states["xyz_l"][i]
    rarm_tf[:3, 3] += states["xyz_r"][i]

    with reach.ENV:
        reach.PR2.SetTransform(base_tf)

        dofvalues = reach.LEFT.FindIKSolution(larm_tf, 2 + 8)
        if dofvalues is not None:
            reach.PR2.SetDOFValues(dofvalues, reach.LEFT.GetArmIndices())
        else:
            print "left ik failed"

        dofvalues = reach.RIGHT.FindIKSolution(rarm_tf, 2 + 8)
        if dofvalues is not None:

Example #10

File: conversions.py Project: warriorarmentaix/lfd

def quat2mat(quat):
    return transformations.quaternion_matrix(quat)[:3, :3]

Example #11

File: test_multi_item.py Project: hojonathanho/python

def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1) 
    quat2_mat = jut.quaternion_matrix(quat2) 
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return (sum([x**2 for x in jut.euler_from_quaternion(diff_quat)]))**0.5

Example #12

File: conversions.py Project: hojonathanho/python

def quat2mat(quat):
    return transformations.quaternion_matrix(quat)[:3, :3]

Example #13

handles.append(reach.ENV.drawlinestrip(points=base_xyzs, linewidth=3.0))

handles.append(reach.ENV.drawlinestrip(points=states["xyz_r"], linewidth=3.0))
handles.append(
    reach.ENV.drawlinestrip(points=states["xyz_l"],
                            linewidth=3.0,
                            colors=(0, 0, 1)))

for i in xrange(len(base_xys)):
    base_tf = np.eye(4)
    base_tf[0:2, 3] = base_xys[i]

    xlarm, ylarm, zlarm = states["xyz_l"][i]
    xrarm, yrarm, zrarm = states["xyz_r"][i]

    larm_tf = tf.quaternion_matrix(states["quat_l"][i])
    rarm_tf = tf.quaternion_matrix(states["quat_r"][i])
    larm_tf[:3, 3] += states["xyz_l"][i]
    rarm_tf[:3, 3] += states["xyz_r"][i]

    with reach.ENV:
        reach.PR2.SetTransform(base_tf)

        dofvalues = reach.LEFT.FindIKSolution(larm_tf, 2 + 8)
        if dofvalues is not None:
            reach.PR2.SetDOFValues(dofvalues, reach.LEFT.GetArmIndices())
        else:
            print "left ik failed"

        dofvalues = reach.RIGHT.FindIKSolution(rarm_tf, 2 + 8)
        if dofvalues is not None: