Adapted from another answer https://answers.ros.org/question/1961... . 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):
map_to_base_trans, map_to_base_rot = 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)
