Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- function quat_resolve_along_different_axes()
- pkg load quaternion
- % start at body aligned to global frame
- q0 = quaternion(1,0,0,0);
- wrist_twist_angle = -90
- q_rot_hand = rot2q([1,0,0], wrist_twist_angle*pi/180);
- % The formula seems to require right multiplications
- % the order of multiplications may change based on
- % quaternion convention (frame representation versus rotation representation?)
- q1 = q0 * q_rot_hand;
- % rotate the result 2 deg along body pitch
- % it should finally show up in global pitch AND yaw
- q2 = q1 * rot2q([0,1,0], 2*pi/180);
- % compute back angular velcoity
- % using formula in
- % https://electronics.stackexchange.com/a/561725
- q3 = inv(q1) * 2 * (q2-q1);
- fprintf('ang vel in body frame\n');
- q3.x * 180/pi
- q3.y * 180/pi
- q3.z * 180/pi
- fprintf('=====================\n\n\n');
- q4 = 2 * (q2-q1) * conj(q1);
- fprintf('ang vel in desk-monitor frame\n');
- q4.x * 180/pi
- q4.y * 180/pi
- q4.z * 180/pi
- fprintf('=============================\n\n\n');
- endfunction
- ======
- RESULT
- ======
- =======================
- wrist_twist_angle = 45
- =======================
- ang vel in body frame
- ans = -2.0267e-15
- ans = 1.9999
- ans = 0
- ang vel in desk-monitor frame
- ans = -2.0267e-15
- ans = 1.4141
- ans = 1.4141
- =======================
- wrist_twist_angle = -90
- =======================
- ang vel in body frame
- ans = 1.5530e-18
- ans = 1.9999
- ans = 0
- ang vel in desk-monitor frame
- ans = 1.5530e-18
- ans = 1.9878e-16
- ans = -1.9999
- =======================
- wrist_twist_angle = 90
- =======================
- ang vel in body frame
- ans = -1.5530e-18
- ans = 1.9999
- ans = 0
- ang vel in desk-monitor frame
- ans = -1.5530e-18
- ans = 1.9878e-16
- ans = 1.9999
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement