Here, the Lynx6 manipulator Jacobian (using your link lengths) will be used to follow a trajectory. Basically, given $\alpha(0)$ and the body manipulator Jacobian, one can use resolved rate trajectory generation to follow a given desired trajectory.

For this problem, attempt to generate a trajectory from the initial position $Pos(g_i) = d_i$ (given in previous spline problem) that moves to the final location $Pos(g_f) = d_f$ in the same amount of time as given for the spline problem. Since the initial joint values $\alpha$ were known, the start condition is known. The trajectory should then flow according to the linear velocity $v$ (in the world frame) for the allotted time such that it ends up at $d_f$. To work for the manipulator (body) Jacobian, this velocity should be converted from the world frame to the body frame via the inverse transformation $[g_e(\alpha(t))]^{-1}$ as the manipulator moves. Turn in the time varying signal $\xi(t)$ that gets computed along the trajectory for the constant velocity path (in world frame) to follow (the angular velocity should be zero, so turning in the linear body velocity is sufficient).

Using the past resolved rate trajectory generation code for piktul as an example, add the necessary functions that will do so for the lynx6. Call the function genTrajectoryPos where it is assumed that some kind of $E(3)$ function of time and velocity function of time is given in a structure built with traj.pvec, traj.pdot, and traj.tspan. In reality, you will probably only need the velocity structure field, not the configuration structure field. The output of the system should be a set of joint angles versus time that will move the arm to follow the specified position 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 and angular) body velocity Jaboain matrix for 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). For sure you will also 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. Note that since only the position should be followed, all that you will need is the top three rows of the Jacobian for the position part. Use the pinv Matlab function or the proper slash command (is it forward or backslash for over-determined systems?).

1. Let's check to see if the manipulator is really following the trajectory. Plot the end-effector position as a function of time, as well as the straight line path. They should be close, if not equal. Plot both the position as a function of time, as well as the position as a parametric plot.

2. Plot the joint angles versus time.

3. As requested, make sure to turn in the desired body velocity versus time. The body velocity can be provided as two plots, one for the linear velocity and one for the angular velocity. Plotting together can be troublesome since the units differ (unless ``plotyy``` is used and the plot styles vary). However, the angular velocity should be zero, thus turning in only the linear velocity is sufficient.

4. Demo the lynx6 manipulator following the desired end-effector position trajectory.

Tip: You can always use the lynx6_display function to test out the code. Of course, you will need to have the $SE(3)$ class available and fleshed out enough. If you can get the resolved rate code working to generate the joint angles versus time, then using lynx6_display in a loop will display it as a movie in a Matlab figure. Remember to include the drawnow command at the end of the loop to force the figure to refresh.

———

Back – Main