v0.1.11

robogauge/utils/math_utils.py

@@ -16,3 +16,13 @@ def get_projected_gravity(quat):
     gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz)
 
     return gravity_orientation
+
+def quat_rotate_inverse(q, v):
+    q = np.array(q, np.float32)
+    v = np.array(v, np.float32)
+    q_w = q[0]
+    q_vec = q[1:]
+    a = v * (2.0 * q_w ** 2 - 1.0)
+    b = np.cross(q_vec, v) * q_w * 2.0
+    c = q_vec * np.dot(q_vec, v) * 2.0
+    return a - b + c