《Robotics, Vision and Control — Fundamental Algorithms in MATLAB》第二章课后习题

前言

本博客主要是对《Robiotics，Vision and Control — Fundamental Algorithms in MATLAB》第二章的课后习题进行总结，与大家一起学习交流。

1.Explore the effect of negative roll, pitch or yaw angles. Does transforming from RPY angles to rotation matrix then back to RPY angles give a different result to the starting value as it does for Euler angles?

fprintf('\n Question 1:\n');
f_origin=eye(3,3);
rpy=[-0.1,-0.2,0.3]
rpy2R = rpy2r(-0.1,-0.2, 0.3) % rpy-> R, 'XYZ'
R2rpy = tr2rpy(rpy2R)        %R -> rpy
trplot(f_origin,'color', 'r','frame','O');hold on;
trplot(rpy2R, 'color', 'g','frame','1');

Answer:
As for Euler angles, we kown that mapping from rotation matrix to Euler angle is not unique. But there will not have multiple solutions from rotation matrix to roll-pitch-yaw angle, thanks to roll-pitch-yaw sequence allows all angles to have arbitrary sign. Unfortunately, roll-pitch-yaw has a singularity when ${\theta }_p=\pm \frac{\pi }{2}$