Revision history [back]

Adapted from another answer https://answers.ros.org/question/196149/how-to-rotate-vector-by-quaternion-in-python/. I used this to transform my velocity from global coordinates (so velocity in the north direction, east directions, and rotation about the z-axis, which does not change from global to base coordinates) to the coordinates of the base-frame (base_footprint)

# rotate vector v1 by quaternion q1 
def qv_mult(q1, v1):
    #v1 = t.unit_vector(v1)
    q2 = list(v1)
    q2.append(0.0)
    return t.quaternion_multiply(
        t.quaternion_multiply(q1, q2), 
        t.quaternion_conjugate(q1)
    )[:3]

#twist_rot is the velocity in euler angles (rate of roll, rate of pitch, rate of yaw; angular.x, angular.y, angular.z)
#twist_vel is the velocity in cartesian coordinates (linear.x, linear.y, linear.z)
def transform_twist(twist_rot, twist_vel):

    target_frame = "/base_footprint"
    source_frame = "/map"
    map_to_base_trans, map_to_base_rot = listener.lookupTransform(target_frame, source_frame, rospy.Time());

    q1 = map_to_base_rot
    out_vel = qv_mult(q1, twist_vel)
    out_rot = twist_rot

    return out_vel, out_rot
#out_vel is the linear velocity w.r.t to the base_footpring, linear.x, linear.y, linear.z
#out_Rot is the angular velocity w.r.t. to the base_footprint, angular.x, angular.y, angular.z
out_vel, out_rot = transform_twist(twist_rot, twist_vel)

Adapted from another answer https://answers.ros.org/question/196149/how-to-rotate-vector-by-quaternion-in-python/. I used this to transform my velocity from global coordinates (so velocity in the north direction, east directions, and rotation about the z-axis, which does not change from global to base coordinates) to the coordinates of the base-frame (base_footprint)

# rotate vector v1 by quaternion q1 
def qv_mult(q1, v1):
    #v1 = t.unit_vector(v1)
    q2 = list(v1)
    q2.append(0.0)
    return t.quaternion_multiply(
        t.quaternion_multiply(q1, q2), 
        t.quaternion_conjugate(q1)
    )[:3]

#twist_rot is the velocity in euler angles (rate of roll, rate of pitch, rate of yaw; angular.x, angular.y, angular.z)
#twist_vel is the velocity in cartesian coordinates (linear.x, linear.y, linear.z)
def transform_twist(twist_rot, twist_vel):

    target_frame = "/base_footprint"
    source_frame = "/map"
    map_to_base_trans, map_to_base_rot = listener.lookupTransform(target_frame, source_frame, listener.lookupTransform("map", "base_footprint", rospy.Time());

    q1 = map_to_base_rot
    out_vel = qv_mult(q1, twist_vel)
    out_rot = twist_rot

    return out_vel, out_rot
#out_vel is the linear velocity w.r.t to the base_footpring, linear.x, linear.y, linear.z
#out_Rot is the angular velocity w.r.t. to the base_footprint, angular.x, angular.y, angular.z
out_vel, out_rot = transform_twist(twist_rot, twist_vel)