Guest User

Untitled

a guest
Jul 11th, 2020
24
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import numpy
  2. from transformations import quaternion_multiply, quaternion_conjugate
  3.  
  4. def skew_symmetric(q_d):
  5. S = numpy.array([
  6. [0.0, -q_d[2], q_d[1]],
  7. [q_d[2], 0.0, -q_d[0]],
  8. [-q_d[1], q_d[0], 0.0]])
  9. return S
  10.  
  11. quaternion = numpy.array([4, -25, 45, 2]) / numpy.linalg.norm([4, -25, 45, 2])
  12. desired = numpy.array([0.71, 1, 0.71, -5]) / numpy.linalg.norm([0.71, 1, 0.71, -5])
  13. print(quaternion_multiply(desired, quaternion_conjugate(quaternion)))
  14. print(-desired[0] * quaternion[1:] + quaternion[0] * desired[1:] - numpy.matmul(skew_symmetric(desired[1:]), quaternion[1:]))
  15. print(-desired[0] * quaternion[1:] + quaternion[0] * desired[1:] - numpy.dot(skew_symmetric(desired[1:]), quaternion[1:]))
  16. print(-(desired[0] * quaternion[1:] - quaternion[0] * desired[1:] + numpy.matmul(skew_symmetric(desired[1:]), quaternion[1:])))
  17. print(-(desired[0] * quaternion[1:] - quaternion[0] * desired[1:] + numpy.dot(skew_symmetric(desired[1:]), quaternion[1:])))
RAW Paste Data