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 with a constant body velocity $\xi$ 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. Of course, this velocity should be converted from the world frame to the body frame via the inverse transformation $\inverse{g_e}(\alpha(t))$ as the manipulator moves. Turn in the time varying signal $\xi(t)$ that gets computed along the trajectory for the constant veloity path (in world frame) to follow.

Using the past resolved rate trajectory generation code for {\tt piktul} as an example, add the necessary functions that will do so for the {\tt lynx6}. Call the function {\tt 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 {\tt traj.pvec}, {\tt traj.pdot}, and {\tt 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 {\tt ode45} and should be in a structure with the fields as per: {\tt jtraj.alpha} and {\tt jtraj.time}.

You will also need to write a function called {\tt 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 {\tt 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 {\tt pinv} Matlab function or the proper slash command (is it forward or backslash for overdetermined systems?).

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.

{\em Tips:} You can always use the {\tt lynx6\_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 {\tt lynx6\_display}
in a loop will display it as a movie in a Matlab figure.  Remember to
include the {\tt drawnow} command at the end of the loop to force the
figure to refresh.

———

Back – Main