Advertisement
Guest User

resolving angular velocity in different frames

a guest
May 19th, 2021
103
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Octave 1.76 KB | None | 0 0
  1. function quat_resolve_along_different_axes()
  2.   pkg load quaternion
  3.  
  4.   % start at body aligned to global frame
  5.   q0 = quaternion(1,0,0,0);
  6.  
  7.   wrist_twist_angle = -90
  8.   q_rot_hand = rot2q([1,0,0], wrist_twist_angle*pi/180);
  9.  
  10.   % The formula seems to require right multiplications
  11.   % the order of multiplications may change based on
  12.   % quaternion convention (frame representation versus rotation representation?)
  13.   q1 = q0 * q_rot_hand;
  14.  
  15.   % rotate the result 2 deg along body pitch
  16.   % it should finally show up in global pitch AND yaw
  17.   q2 = q1 * rot2q([0,1,0], 2*pi/180);
  18.  
  19.   % compute back angular velcoity
  20.   % using formula in
  21.   % https://electronics.stackexchange.com/a/561725
  22.   q3 = inv(q1) * 2 * (q2-q1);
  23.  
  24.   fprintf('ang vel in body frame\n');
  25.   q3.x * 180/pi
  26.   q3.y * 180/pi
  27.   q3.z * 180/pi
  28.   fprintf('=====================\n\n\n');
  29.  
  30.   q4 = 2 * (q2-q1) * conj(q1);
  31.   fprintf('ang vel in desk-monitor frame\n');
  32.   q4.x * 180/pi
  33.   q4.y * 180/pi
  34.   q4.z * 180/pi
  35.   fprintf('=============================\n\n\n');
  36. endfunction
  37.  
  38.  
  39. ======
  40. RESULT
  41. ======
  42. =======================
  43. wrist_twist_angle =  45
  44. =======================
  45. ang vel in body frame
  46. ans =   -2.0267e-15
  47. ans =  1.9999
  48. ans = 0
  49.  
  50. ang vel in desk-monitor frame
  51. ans =   -2.0267e-15
  52. ans =  1.4141
  53. ans =  1.4141
  54.  
  55.  
  56. =======================
  57. wrist_twist_angle = -90
  58. =======================
  59. ang vel in body frame
  60. ans =    1.5530e-18
  61. ans =  1.9999
  62. ans = 0
  63.  
  64. ang vel in desk-monitor frame
  65. ans =    1.5530e-18
  66. ans =    1.9878e-16
  67. ans = -1.9999
  68.  
  69.  
  70. =======================
  71. wrist_twist_angle =  90
  72. =======================
  73. ang vel in body frame
  74. ans =   -1.5530e-18
  75. ans =  1.9999
  76. ans = 0
  77.  
  78.  
  79. ang vel in desk-monitor frame
  80. ans =   -1.5530e-18
  81. ans =    1.9878e-16
  82. ans =  1.9999
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement