Robotics CITS3241

Exercise Sheet 7: Jacobian Matrices and Motion Rate Control.

1. Modify your stanford forward kinematics function, stanford6dof, to match the specification below. With these changes the function will now also calculate a Jacobian matrix

   %  function [T, J] =  stanford6dof(joint, offset, drawmode)
   %
   %  Function for calculating the forward kinematics of a full 6 DOF stanford arm
   %
   %  Input arguments:  joint  - a 6 element array of joint positions
   %                    offset - [offset1 offset6] (The two fixed offsets.)
   %                    drawmode - A parameter that replaces/augments 'coordframe'
   %                               0  - Draw rendered image of arm
   %                               1  - Draw rendered image of arm + joint coordinate frames
   %                               2  - Draw nothing, just calculate T and J
   %
   %  Return values:    T     - the homogeneous transform that describes the
   %                            end-effector frame
   %                    J     - the 6x6 Jacobian matrix corresponding to the
   %                            current arm configuration.
   %
   
   

   Note that MATLAB provides a function cross which calculates the cross product between two vectors.
   
   

2. Write a function that matches the following specification

   % function finaljoint = stanfordmrcntrl(joint, offset, CP, dt)
   %
   % Function for performing motion rate control of a stanford 6 dof arm
   % along a series of cubic Bezier curves.
   %
   % (At this stage) This function only controls the position of the end effector,
   % not its orientation. The orientation of the end effector remains constant
   % through the movement, that is, the orientation velocity is [0 0 0] at all
   % times.
   %
   % Input arguments: joint  - a 6 element array of joint positions
   %                  offset - [offset1 offset6] (The two fixed offsets.)
   %                  CP    - a nx3 vector of n Bezier control points in 
   %                          3-space eg.                    [x y z;
   %                                                          x y z;
   %                                                            ... ]
   %                  dt    - increment in the parameter along the Bezier curve
   %                          that is made between successive movements.
   %
   % Return value:    finaljoint - a 6 element array of the joint positions at
   %                               the final position of the arm.
   %
   % Control of the robot proceeds as follows:
   %
   % The forward kinematics are evaluated on the initial joint angles to
   % determine the starting end effector position and orientation.
   %
   % The array of control points, CP, form a series of cubic Bezier curves
   % that have been spliced together.  Note however, that CP should *not*
   % include the first two control points of the first curve.  The start of
   % the curve has to be made to match the robot's initial position.  We do
   % this by constructing two Bezier control points, corresponding to the
   % robot's initial position, and prepend these to the array of control
   % points.  This construct a Bezier curve that starts at the robot's
   % current position (at zero velocity), and then leads into the desired
   % curve that was specified by the original array of control points.
   %
   % The robot then proceeds to step along the Bezier curve in parameter 
   % increments of dt, using motion rate control, until it reaches the end 
   % of the curve.
   

    dx = J*dtheta

    dtheta = J\dx;

    dtheta = inv(J)*dx;

When testing your motion rate control code it is possible that you will run into singularities (a common cause is theta5 being close to 0 or pi). To monitor the condition number of the Jacobian matrix you can use the MATLAB function cond (when the condition number becomes large, say > 100, things are bad). Print out the condition number and the joint angles on each step - you will see when you start to run into trouble. It can be a bit of an art devising trajectories that do not run into singularities.

Portfolio Note