Here, the Lynx6 manipulator Jacobian (using your link lengths) will be used to follow a trajectory. For this problem, attempt to generate a trajectory from the initial $g_i$ from the spline problem that moves with a constant body velocity $\xi$ to the final location $g_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 velocity $\xi$ for the allotted time.
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 genTrajectory where it is assumed that some kind of $SE(3)$ function of time and body 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 body 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 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 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). 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: Probably you will notice we are in the kinematically insufficient case, for which I said one should never try to do the psuedo-inverse since it will be wrong always. That is the case here, regarding being wrong. Probably I should just be doing resolved rate trajectory generation with the end-effect position and ignoring the orientation as for the previous resolved rate implementation, or making sure that my desired trajectory is legit. For the sake of this problem, let's just ignore this untidy fact and pretend it's all good.
1. Turn in the value $\xi$ computed.
2. Since this manipulator is kinematically insufficient, it most likely won't be able to follow the exponential trajectory. You can see this by printing the original final end-effector configuration $g_e(\alpha_f)$ and the one from the resolved rate trajectory $g_e(\alpha(t_f))$. Compute their difference by doing $g_{err} = [g_e(\alpha(t_f))]^{-1} g_e(\alpha_f)$ and then taking the logarithm to get $\xi_{err}$. Turn in all of the quantities described here. What is the norm of $\xi_{err}$? That gives an indication of how close the two are.
3. Demo the final generated trajectory with the lynx6 manipulator.
The extra calculations in [1] are to see how wrong the final configuration is from the one that it should be (the elbow manipulator would be able to follow the trajectory no problems since it is kinematically sufficient).
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.
———