CS 148 Project 7: Inverse Kinematics

The Canadarm2 on the International Space Station during the STS-114 space shuttle mission. Robotic arms like the Canadarm2 use inverse kinematics to position its endeffector at a specific location.

This assignment is intended to let you experiment with inverse kinematics using MATLAB simulations.

Inverse kinematics is the problem of determining the configuration necessary to achieve a desired end effector position. This the the inverse problem of forward kinematics, where the end effector position is calculated based on the arm configuration. In general, with inverse kinematics, there is not a unique answer. (Consider your own arm, for instance. You can keep your hand (end effector) in the same place while rotating your elbow in many locations.)

In this assignment, you will fill in removed sections of a basic inverse kinematics engine.

Fill in the missing pieces! After you have completed this, your engine will be able to run the same demos you saw in class previously.

Assigned: Monday Nov. 28, 2011

Due: Saturday Dec. 10, 2011

Your tasks for the milestone are:

1. Complete the IK engine.
2. Test the IK engine.

Various blanks have been left in the code. Each has been marked with a %TODO. Fill these in as appropriate. You can assume that outside of these blanks, all of the support code functions correctly. (We know this is true, because we created this assignment by removing sections of code from a working engine.)

A brief overview of code organization is as follows:

• initialization: Initialize the joint offsets, the initial joint angles, and the goal position
• iterate: Each iteration will compute the current arm positions using forward kinematics, draw the arm, and compute the next step using inverse kinematics.
  • construct transforms: The rotation and translation matrices which convert each link from it's own coordinate space into world space (ie, “workspace”) are computed.
  • traverse forward kinematics: The transformation matrices just constructed are traversed to draw the forward kinematic chain.
  • additional drawing updates: As it's name would suggest. You don't have to do anything here.
  • inverse kinematics: Construct the Jacobian, and solve the equation “linvel = J * angvel” for angvel using two different methods, namely the Jacobian transpose and the pseudo-inverse.
  • finish iteration: Drawing updates. Again, you don't have to do anything here.

See the next section (Submission and Testing) for details on how we'll test your code. We have the bar set pretty high for performance on this assignment, so we recommend that you try to find and test corner cases, perform random testing within the domain space, and the like. To get you started, we have included several examples. The results of running each of them against the code's key are below:

• testik(1,1)

• testik(1,2)

• testik(2,1)

• testik(2,2)

• testik(3,1)

• testik(3,2)

• testik(4,1)

• testik(4,2)

• testik(5,1)

• testik(5,2)

To submit your code, create the branch “inverse_kinematics_[your_team_name]”. The course staff will check out and run this code without modification. If you have anything out of the ordinary, include a README file to explain it. This code will be tested against configurations similar to the test code. To receive a check for the assignment, your controller must successfully complete 90% of trials.

We will test your engine with parameters structured similarly to the support code's provided example tests. We will call your code in a way similar to these tests. (In other words, if you can run the examples in the support code, and you didn't do anything goofy to the support code, you're fine as far as having your engine called.)

Finally, take a deep breath. You're done! We hope you've learned a lot about robotics both in theory and in practice this semester, and we hope you've enjoyed participating in the course as much as we've enjoyed teaching it. Robotics is still a very new and exciting field of study, and we suspect it will grow greatly over the next several decades. It's an exciting time for us, and we'd love you to be a part of it!

There will be a help session during class time (1pm-1:50pm) on Friday, Nov. 02 on basic MATLAB operations. Please plan on attending if you're unfamiliar or rusty with MATLAB. The amount of the language you will need to use for this assignment is minimal (relative to the amount of MATLAB out there), so you shouldn't have much trouble picking up what you need to for this assignment.

Second, use TA hours! We're here to help!

• For loops

for i=1:5
    i;
end

• If statements

if i == 4
    i = i+1;
end

• Comments

% Comment!
%% Section! (You can use ctrl+Enter to run a section in the MATLAB editor.  The section starts at the %% and ends at the next %%.)

• Semicolons at the end of a line suppress output.

• Ellipses (…) at the end of a line allow you to continue a statement on the next line.

• Basic matrix use/allocation

a = [1,2,3;4,5,6;7,8,9]; % Comma -> next column; Semicolor -> next row
a(2,3) % Indexing starts at 1, not 0
a(1,:) % Everything in row 1
a(:,2) % Everything in column 2
a(1:2,2:3) % Rows 1-2, Columns 2-3
a(:,:) % The entire matrix
a(:) % The entire matrix as a single column.  (MATLAB stores matrices in column-major order.)
a(4) % (Again, column-major order.)
a(5,5) = 100; % Matrices also expand dynamically!

• Variable allocation (python-like)

hey_its_a_new_variable = 1
hey_its_another_new_variable = 2
hey_its_another_new_variable = 3

• Size

b = [1,2;3,4;5,6];
size(b) % Returns [num_rows, num_cols]
size(b,1) % Number of rows
size(b,2) % Number of columns

• Transpose

b = [1,2;3,4;5,6];
b = b'; % b is now [1,3,5;2,4,6]

• Inverse

inv(a) % Note that a must be a square matrix

• Matrix addition/subtraction/multiplication

b = [1,2,3;4,5,6];
c = [5,6;7,8;9,0];
a = c * b; % Just like you'd write it!
d = [3,5,6;4,5,2];
e = d + b;
f = d - b;

• Useful functions (ones/zeros/eye)

ones(num_rows, num_cols); % Matrix of ones
zeros(num_rows, num_cols); % Matrix of zeros (good for preallocation)
eye(n); % Identity matrix

• Structs (javascript-like)

foo.bar = 1;
foo.baz = 2;
foo.qux = [5,6;7,8];

(Note: In your assignment, you have the phrase T(i,i+1).transform. T is just a (relatively sparse) matrix of structs, with each struct containing rotation, translation, and transform matrices.)

The skeleton code is below. If you are copy/pasting it to a file, you will need to save it as testik.m .

%
% Forward and inverse kinematics example : n-link 2D arm
%
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: October 7, 2011
%
% based on Stefan Schaal's lecture slides and Watt&Watt textbook
%
% call as function with 2 arguments:
% xmpl: switch variable for preset examples (1-12)
% ikmap: method of IK (1: Pseudo Inverse, default: Jacobian Transpose)
%

function [dontcare] = testik(xmpl,ikmap)


%----------------------------
%% initialization

% ---- default example ----
% set relative positions of links with respect to parent joint
joint_pos_local = [[2;0] [1;0] [1;0] [1;0] [1;0]];
% initialize joint angle configuration
q = [30*pi/180 -45*pi/180 90*pi/180 45*pi/180];
% initialize desired endeffector position in workspace
xd = [-3 1.5 1]'; % using homogenous coordinates

% set configuration and endeffector parameters
if (xmpl == 1)  % point on the ground
    joint_pos_local = [[2;0] [1;0] [1;0] [1;0] [1;0]];
    q = [80*pi/180 -45*pi/180 90*pi/180 45*pi/180];
    xd = [-3 0 1]';
elseif (xmpl == 2)  % weird offsets
    joint_pos_local = [[2;0.5] [1;-0.25] [1;0]];
    q = [-75*pi/180 15*pi/180];
    xd = [3 -3 1]';
elseif (xmpl == 3)  % touch the base
    joint_pos_local = [[2;0] [1;0] [1;0] [1;0]];
    q = [80*pi/180 -45*pi/180 45*pi/180];
    xd = [0 0 1]';
elseif (xmpl == 4) % reach high
    joint_pos_local = [[2;0] [1;0] [1;0] [1;0] [1;0]];
    q = [30*pi/180 -45*pi/180 90*pi/180 45*pi/180];
    xd = [0 5.9 1]';
elseif (xmpl == 5) % reaching too high
    joint_pos_local = [[2;0] [1;0] [1;0] [1;0] [1;0]];
    q = [30*pi/180 -45*pi/180 90*pi/180 45*pi/180];
    xd = [0 8 1]';
end

% set IK step size
if (ikmap ~= 1)
    ik_stepsize = 0.03;
else
    ik_stepsize = 0.2;
end

% initialize drawing figure
figure;
fill([3 -3 -3 3],[0.1 0.1 -0.5 -0.5],'m');
hold on;
plot(xd(1,:),xd(2,:),'k.','MarkerSize',40);
plot(xd(1,:),xd(2,:),'g.','MarkerSize',30);


%----------------------------
%% iterate arm joint angles (q) to move towards desired endeffector location (xd)
while (1)


    %----------------------------
    %% construct transforms between each joint connecting parent and child bodies
    %  based on current configuration
    for i = 1:size(q,2)
	%TODO: Change this matrix.  Recall q is the rotation of each joint angle.
	T(i,i+1).rotate = ...
	    [1, 0, 0;
	     0, 1, 0;
	     0, 0, 1];

	%TODO: Change this matrix.  Recall joint_pos_local describes the position
	%      of each joint angle..
	T(i,i+1).translate = ...
	    [1, 0, 0;
	     0, 1, 0;
	     0, 0, 1];

	%TODO: The combined transform uses the rotation and translation matrices.
        T(i,i+1).transform = ...
	    [1, 0, 0;
	     0, 1, 0;
	     0, 0, 1];
    end;


    %----------------------------
    %% traverse forward kinematics to place each link in workspace (includes drawing links)
    joint_pos_world = zeros(3,size(joint_pos_local,2));
    curtrans = eye(3); % Initialize current matrix stack with identity
    for i = 1:size(q,2)
        %TODO: Compute the world space position of the joint (That is, the
	%       Homogeneous coordinates stored as a translation in curtrans).
        joint_pos_world(:,i) = [0; 0; 1];

        % Transform and draw current link geometry to workspace
        joint_geom_local = [[-.2 -.2 1]' [joint_pos_local(1,i)+.2 -.2 1]' [joint_pos_local(1,i)+.2 joint_pos_local(2,i)+.2 1]' [-.2 joint_pos_local(2,i)+.2 1]' ];
        joint_geom_world = curtrans * T(i,i+1).rotate * joint_geom_local;
        plot(joint_geom_world(1,:),joint_geom_world(2,:));

        %TODO: Transform the coordinate system to the next link.  (What's the
	%      difference between curtrans and the next transform?
        curtrans = eye(3);

    end

    % transform and draw last link geometry to workspace
    joint_geom_local = [[-.2 -.2 1]' [joint_pos_local(1,size(q,2)+1)+.2 -.2 1]' [joint_pos_local(1,size(q,2)+1)+.2 joint_pos_local(2,size(q,2)+1)+.2 1]' [-.2 joint_pos_local(2,size(q,2)+1)+.2 1]'  ];
    joint_geom_world = curtrans * joint_geom_local;
    joint_pos_world(:,size(joint_pos_local,2)) = curtrans * [0 0 1]';
    plot(joint_geom_world(1,:),joint_geom_world(2,:));

    % transform and draw endeffector to workspace
    endeffector_pos = curtrans * [joint_pos_local(1,size(q,2)+1) joint_pos_local(2,size(q,2)+1) 1]';
    plot(endeffector_pos(1,:),endeffector_pos(2,:),'k.','MarkerSize',40);


    %----------------------------
    %% additional drawing updates

    axis('equal');

    if (1==1) %draw connections between joints
        plot(joint_pos_world(1,:),joint_pos_world(2,:));
        hold on;
        plot(joint_pos_world(1,:),joint_pos_world(2,:),'r.');
        axis('equal');
    end

    % draw endeffector again
    plot(xd(1,:),xd(2,:),'k.','MarkerSize',40);
    plot(xd(1,:),xd(2,:),'g.','MarkerSize',30);


    %----------------------------
    %% inverse kinematics: iteratively move endeffector to desired location using Jacobian

    %TODO: Construct Jacobian (already transposed)
    %      Note that this means that each row should contain a single v, where
    %      v = w cross r for a normalized r, where
    %      v = the change in joint position (ie, joint velocity)
    %      w = the change in the link's angle (ie, the angular velocity
    %          pseudovector.  Direction is the axis of rotation, and magnitude
    %          is the speed.)
    %      r = the link itself.
    %      Note that here, the axis of rotation should be pointing outward from
    %      screen.
    %      See: http://en.wikipedia.org/wiki/Angular_velocity#Particle_in_three_dimensions
    J = ones(size(q,2), 2);

    % Transform vector to desired endeffector location (deltax) to c-space
    %   displacement (deltaq) using step size ik_stepsize.
    deltax = (xd-endeffector_pos);
    if (ikmap ~= 1)
      %TODO: Use the Jacobian transpose (assume already transposed)
      deltaq = zeros(size(q,2),1);
    else
      %TODO: Use the pseudo-inverse (assume J already transposed)
      deltaq = zeros(size(q,2),1);
    end

    %TODO: Add the IK c-space displacement to the current configuration
    q = q + zeros(size(q,2),1)';


    %----------------------------
    %% finish iteration

    % update display
    refresh;
    drawnow;

    % wait for next iteration
    pause(1);
    %input('continue');
end