Compute the forward kinematics of the Piktul manipulator. The home configuration should have the robot arm straight out and up at some height. Doing so defines the zero transformation for the rotational joint variables. The linear joint coordinates should go to whatever value is specified (for these zero as a length should be sensible). Most of the piktul robotic arms can't quite manage a zero gripper height (except for one or two of the super-saggy ones), and it is best not to let the gripper finger width go to zero either.

1. Provide the solution to the forward kinematics in symbolic form. Since it is roughly planar, the solution can be written as what I would call $SE(2) \times \mathbb{R}$, which is the planar special Euclidean space plus the height (lies in $\mathbb{R}$).
2. Program the forward kinematics into the piktul. There should be an empty member function for the forward kinematics. It might be missname, so fix the name if that is the case (should be called forwardkin). There should be two return arguments [g, z] so that it returns an $SE(2)$ object (the g part) plus a $\mathbb{R}$$ object (the height z).
3. Take the testCalib code file and modify it so that there is a variable called fingerWidth that can be set to some value. All the finger/gripper widths opening widths should be changed so that they are set to fingerWidth. Aso modify the height so that it remains at a constant $1$ inch the whole time. Then insode of the for loop, add an invoction of the piktul forward kinematics function and display the reslts of the $SE(2)$ part only. Make sure to change the function's name to testForwardKin.m so that it is different from the testCalib01.m file.

You will have to demo this new part. During the demo, we will look at the actual position and orientation values versus the ones predicted by the forward kinematics. They should be pretty close, if not equal.

1. In an earlier homework, you had to integrate the differential equation associated to a plnar to-link manipulator. Perform the integration for a short amount of tim (say, $4$ seconds) and apply the output to the actual manipulator.

Since the system only gives two joint angles as outputs, but the piktul expects a 5×1 vector as input, you can set the other variables constant (the shoulder height, the gripper rotate joint, and the gripper width). This will involve prepending and appending the constant values to the vectors returns from the ode45 function. If you include the pauses like in the homework solution, then it should run in real time. For reasons that I will explain later, I would recommend that you change the execute time to be a little bit longer than the actual pause time. The piktul setArm command should accept a second argument that is the expected duration of the command. So, in the for loop, the lines of code would look something like:

    robot.setArm( alphaNow, dtNow * 1.1);
    pause(dtNow)
  after setting the ''alphaNow'' and ''dtNow'' variables properly.
  

* Note: Inside of the piktul class file, there is a definition of the link lengths already. You should trust that they are OK, although if you'd like to double-check them, that is fine.