7.1. Forward Kinematics Homework¶

Write a MATLAB script to find the pose of the end effector for a simple two link robot operating on a plane. The first joint angle is $\pi/3$ . The length of the first arm is 12 cm. The second joint angle is $-2\pi/3$ . The length of the second arm is 6 cm. Make a simple plot of the robot using the trplot() function and the following function to draw a line between joints of the robot.
```
function plotArm(T1, T2)
% PLOTARM  -- plot a line pose T1 to pose T2 (both homogenious matrix)

    x1 = T1(1,3);
    y1 = T1(2,3);
    x2 = T2(1,3);
    y2 = T2(2,3);
    line([x1 x2], [y1, y2], 'LineWidth', 3)
end
```
Write a MATLAB script to find the 3-D (Roll, Pitch, Yaw) pose of the end effector described as follows. The first joint angle is at $(0, -\pi/4, \pi/4)$ . The first arm length is 10 cm. The second joint angle is at $(\pi/6, \pi/5, 0)$ . The length of the second arm is 7.5 cm. The third arm is at position $(0, \pi/3, 0)$ . The length of the third arm is 3 cm. Make a simple plot of the robot. Modify the previous plotArm function to plot in 3 dimensions. You may want to rotate the plot to better see each arm of the robot.