sr_move_arm: sr_move_arm: quaternion

00001 #!/usr/bin/env python
00002 
00003 class Quat:
00004     def __init__(self):
00005         self.a = 0
00006         self.b = 0
00007         self.c = 0
00008         self.d = 0
00009 
00010 def mult(q1,q2):
00011     result = Quat()
00012     result.a = (q1.a*q2.a -q1.b*q2.b -q1.c*q2.c -q1.d*q2.d)
00013     result.b = (q1.a*q2.b +q1.b*q2.a +q1.c*q2.d -q1.d*q2.c)
00014     result.c =  (q1.a*q2.c -q1.b*q2.d +q1.c*q2.a +q1.d*q2.b)
00015     result.d = (q1.a*q2.d +q1.b*q2.c -q1.c*q2.b +q1.d*q2.a)
00016     return result
00017 
00018 def display(q):
00019     print "Quaternion: "
00020     print q.a
00021     print q.b
00022     print q.c
00023     print q.d
00024     print "---"
00025 
00026 q1 = Quat()
00027 q1.a = 0.406122
00028 q1.b=-0.363322
00029 q1.c = 0.0986163
00030 q1.d= -0.832669
00031 
00032 q2 = Quat()
00033 q2.a = 0.48296291314453427
00034 q2.b = -0.2241438680420133
00035 q2.c = 0.8365163037378078
00036 q2.d = 0.1294095225512604
00037 
00038 
00039 q3 = mult(q1,q2)
00040 
00041 display(q3)
quaternion_compo.py
