Robotics CITS3241

Exercise Sheet 5: Six Degrees of Freedom

The Denavit-Hartenberg parameters for a 6 Degree of Freedom Stanford like Arm are as follows

link	variable	theta	offset	length	twist
1	theta1	theta1	offset1	0	pi/2
2	theta2	theta2	0	0	-pi/2
3	offset3	0	offset3	0	0
4	theta4	theta4	0	0	-pi/2
5	theta5	theta5	0	0	pi/2
6	theta6	theta6	offset6	0	0

The twist angles for links 4 and 5 were chosen to make the wrist rotations correspond closely to the sequence of rotations in the Euler transform. The values chosen above give a 'zero' position as shown below.

1) Write a MATLAB M file called stanford6dof

%  STANFORD6DOF
%
%  Usage:   stanford6dof(joint, offset, coordframe)
%
%  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.)
%                   coordframe - same as for stanford3dof
%  Return value:    T     - the homogeneous transform that describes the
%                           end-effector frame
%

function T = stanford6dof(joint, offset, coordframe)

2) Write a M file to calculate the inverse kinematics of a stanford arm

%  INVSTANFORD6DOF
%
%  Usage:  joint = invstanford6dof(T, offset)
%
%  Function for calculating the inverse kinematics of a full 6 DOF stanford arm
%  Input arguments: T     - The homogeneous transform that describes the
%                           end-effector frame.
%                   offset - [offset1 offset6] (The two fixed offsets.)
%  Return value:    joint - a 4x6 element array of joint positions.
%                           There will be four possible solutions, each 
%                           consisting of six joint positions. There will be  
%                           two arm solutions involving two theta1 values
%                           separated by pi, and for each of these arm
%                           solutions there will be two wrist solutions.
%                           (Note: ignore any extra solutions that could
%                           be obtained obtained by adding pi to theta1 and 
%                           using a negative offset on link 3)

function joint = invstanford6dof(T, offset)

Suggested approach:

• Given the end effector frame, solve for the wrist centre.
• Given the wrist centre, solve for the arm joint positions.
• Given the arm configuration, calculate the transform between the last link of the arm and the end effector frame.
• Adapt the inverse solution for the Euler transform to solve for the wrist angles, or directly solve for these angles based on the transform in the previous step.

Your function invstanford6dof will require you to write the following two functions specified below.
(INVEULER is optional - since it was not covered in lectures, and you can directly solve for these angles instead.)

% INVEULER
%
% Inverse of Euler transform. Function returns 2 possible arrays of 3
% Euler angles, given a 4x4 transformation matrix T)
%
% Returns euler1 = [phi1, theta1, psi1] - the 1st solution
%     and euler2 = [phi2, theta2, psi2] - the 2nd solution

function [euler1, euler2] = inveuler(T)

% INVHT
%
% Function to invert a homogeneous transformation matrix

function Tinv = invht(T)

Do not use the MATLAB inv function - exploit the fact that the upper left 3x3 sub-matrix is orthonormal

<FONT color=darkgreen )

Portfolio Note

All these functions will be required for your portfolio (INVEULER is optional)