.. _FwdKinHW:

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
   :math:`\pi/3`. The length of the first arm is 12 cm. The second joint
   angle is :math:`-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 :math:`(0,
   -\pi/4, \pi/4)`. The first arm length is 10 cm.  The second joint angle
   is at :math:`(\pi/6, \pi/5, 0)`. The length of the second arm is 7.5 cm.
   The third arm is at position :math:`(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.