Piktul: Resolved Rate End-Effector Movement
In the previous adventure module, the position of the end-effector was controlled to follow a trajectory. Naturally, since the final joint variable controlled the orientation of the end-effector only (e.g., was a pure rotation), it did not figure as part of the resolved rate trajectory.
This time around, let's get it to follow a full end-effector configuration trajectory. It will be the same trajectory, but include rotation about the z-axis. That means we will be dealing with the full $SE(2) \times \mathbb{R}$ configuration. As a realizable increment over the position only version, work with the $SE(2)$ part in its vector form. The $g$ vector should be $g = (x, y, z, \theta)^T$. That means that the trajectory to follow will be: \begin{equation} \dot g = \left[ \begin{matrix} \dot x \\ \dot y \\ \dot z \\ \dot \theta \end{matrix} \right] = \left[ \begin{matrix} v_x(t) \\ v_y(t) \\ v_z(t) \\ v_\theta(t) \end{matrix} \right] \end{equation}
Using the past resolved rate trajectory generation code as an example, add a function that will do the full $SE(2) \times \mathbb{R}$ resolved rate problem for the piktul. Call the function genTrajectory where it is assumed that some kind of $SE(2) \times \mathbb{R}$ function of time and velocity function of time is given in a structure built with traj.gvec, traj.gdot,
and traj.tspan. In reality, you will probably only need the velocity structure field, not the configuration structure field (except maybe to define the initial condition). The output of the system should be a set of joint angles versus time that will move the arm to follow the specified trajectory. It can be in the output format of ode45 and should be in a structure with the fields as per: jtraj.alpha and jtraj.time}.
You will also need to write a function called Jacobian that computes the linear velocity of the end effector (in world coordinates, not body coordinates) and the angular velocity of the end-effector as a function of the $\alpha$ coordinates. For completeness, you should include all
$\alpha$ coordinates in it (the last one will just have a zero column since it doesn't contribute to movement, it just opens/closes the gripper). The implementation should then be
\begin{equation}
\dot a = {\rm pinv}(\ {\rm Jac}(\alpha)\ ) * v(t).
\end{equation}
For sure you will reuse the followJointTrajectory function to make it happen. The script you wrote for the last homework can be recycled and modified to work for this problem.
Tips: You can always use the piktul_display function to test out the code. If you can get the resolved rate code working to generate the joint angles versus time, then using piktul_display in a loop will display it as a movie in a Matlab figure (it's good for debugging at home, then coming in to the lab to simply demo). Remember to include the 'drawnow command at the end of the loop to force the figure to refresh.
Tips: Don't forget that Matlab likes to use radians, but often I specify things in degrees.