﻿

# Untitled

a guest
Jul 11th, 2020
37
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