Introduction to Robotics_ch02