Inverse Kinematics for a General Purpose Robot Arm That Moves in 3D


Now we're going to look at a robot with six joints or six degrees of freedom and this is a robot that moves in three dimensions.  I'm going to start by loading a model for the Puma 560 robot and that creates a number of robots in the workspace.  Most important of these is the variable P560, this is a MATLAB object that represents an object.  There are a few other variables as well and these represent particular joint angle configurations, QZ for instance is a vector of length six with all zeros and it represents the set of zero joint angles for this particular robot. 

The robot objects have got a lot of methods and one of the methods; the plot method, allows us to view the configuration of a robot, so here is that particular robot when all the joint angles are equal to zero.  I can look at this from a number of different viewpoints and see the robot and we can see also the robot end effector co-ordinate frame here showing the X, Y and Z axes of the robot end effector. 

Now we're talking about inverse kinematics so I want the robot to go to a particular pose and I'm going to define that.  First of all I'm going to define it's translational component using the function transl and I want an X co-ordinate of 0.6, a Y co-ordinate of 0.1 and a Z co-ordinate of 0 and now I need to give it some orientation.  And I do that by specifying its role, pitch and yaw angles.  I want the role angle to be 0, I want the pitch angle to be 180 degrees and I want the yaw angle to be 0 degrees and I specify that I have given the angles in terms of unit of degrees.  And here is the homogeneous transformation matrix representing the pose that I'm interested in. 

Now let's have a look at that particular pose and I can superimpose the pose into the figure that contains the robot, enable hold and I now plot that particular pose and then we can see the co-ordinate frame which represents this pose and we can have a look around here.  So the position associated with this pose is the origin of this new blue co-ordinate frame and we can see the orientation of this new frame in terms of its X, Y and Z axes.  Now our job is to determine the joint angles the robot needs to adopt so that its end effector lines up with this new desired pose.  Do that using inverse kinematics.  There is an inverse kinematic method associated with the robot object and the method's name is ikine, which is short hand for inverse kinematics, six, for a six degree of freedom robot and S because it has a spherical wrist.  And the only argument that I pass in is T, the homogeneous transformation representing the desired pose. 

The result is the set of joint angles that if the robot adopted these joint angles, it would achieve the pose.  Well let's test that, we can move our robot to this set of joint angles that we've just computed and we can see here that the robot has definitely moved.  Now let's have a look around in here and see what's happened.  So we can see that the center of the end effector is now lined up with the center of this new co-ordinate frame that we introduced and we can see that the X, Y and Z axes of the robot end effector are lined up with the X, Y and Z axes of the blue frame that we just introduced.  Now it's a little difficult in this stick figure plot, to determine the configuration of this particular robot.  Is this in a left handed configuration or a right handed configuration.  It's really a bit difficult to tell, however there is an improved way of looking at the robot, and that's using the method Plot 3D.  It takes a little moment to get started the first time you run it because it needs to load some detailed CAD models of the robot into the workspace.  What we see here is then a beautifully rendered image of the Puma 560 robot and it's quite clear that the robot is in a left handed configuration and the orientation of its gripper is straight down basically in a configuration where it could pick things up from a horizontal table. 

Now we might be interested in putting the robot into a different configuration. We can do that by re-computing the inverse kinematics, but this time we're going to pass in an option R, which is asking for the joint angles that correspond to a right handed configuration.  Here is the set of joint angles and we can plot those and we can see now that our robot is very clearly in a right handed configuration. 

Now I could also ask for it to be in a right handed configuration with the elbow down rather than up and pass in the option D to indicate that.  We have now a different set of joint angles, you see that they differ to the values shown above and we can plot this and we can see that our robot now is in a right handed configuration but with its elbow down and the wrist is in a very, very odd pose indeed.  It looks quite awkward.  I can also then ask for the robot to be in a left handed configuration with the elbow being down and I can plot that and it looks something like this... 

What we've seen for this six degree of freedom robot that moves in three dimensions, is that there are in fact eight solutions for any particular end effector pose.  There is a solution where the robot is either in a left handed or right handed configuration, there is solutions where the elbow is up, that means that the elbow is higher than the shoulder or where the elbow is down; that's where the elbow is below the shoulder and there's solutions where the wrist is flipped or not flipped, that's a rotation of 180 degrees in the configuration of the wrist.  For the case of the robot with a two fingered gripper, as is shown here, then a rotation of 180 degrees around the wrist axis, leaves two fingers in effectively the same configuration and it makes no difference when it comes to grasping an object, picking it up or putting it down.


There is no code in this lesson.

For real robots such as those with 6 joints that move in 3D space the inverse kinematics is quite complex, but for many of these robots the solutions have been helpfully derived by others and published. Let’s explore the inverse kinematics of the classical Puma 560 robot.

Professor Peter Corke

Professor of Robotic Vision at QUT and Director of the Australian Centre for Robotic Vision (ACRV). Peter is also a Fellow of the IEEE, a senior Fellow of the Higher Education Academy, and on the editorial board of several robotics research journals.

Skill level

This content assumes an understanding of high school level mathematics; for example, trigonometry, algebra, calculus, physics (optics) and experience with MATLAB command line and programming, for example workspace, variables, arrays, types, functions and classes.

More information...

Rate this lesson


Check your understanding


  1. Vibhutha says:

    I spend almost week with Craig’s robotics book to understand how Inverse Kinematics works. If I watched this video first, I would finish that in couple of hours. Thanks to your great explanation and matlab toolbox.

    1. Peter Corke says:

      Share this site with your classmates! Are you using Craig’s new fourth edition?

  2. jose sepulveda says:

    What math level is required to do this?

    1. Peter Corke says:

      Check out the skill level, below the video.

  3. Bidhan says:

    p560.plot3d(q) gives an error message saying ‘Index exceeds matrix dimensions’. Please help.

    1. Peter Corke says:

      These questions probably better off discussed http://tiny.cc/rvcforum. Check that q is a 1×6 matrix.

  4. damianleporis says:

    Which version of rvc toolbox are you using for recording videos? I have 9. 10 because plot function in 10.3 is not working at all for me. But I still keep getting this error message in 9.10:
    Undefined function or variable ‘rmiml.visibleInToolstrip’.

  5. Sachin Nath says:

    Sir can you explain how the robot is in left handed configuration at 4:16

    1. Sachin Nath says:

      I got it sir.

    2. Peter Corke says:

      Imagine it was a person there instead of a robot, what you see is their left arm.

  6. Filip Stojanovic says:

    I have a problem with this code :
    T=transl(0.5, 0.5, 0)*rpy2tr(0, 180, 0)
    hold on
    q=p560.ikine6s(T, ‘r’)
    It plots the puma560 robot nicely, but when i try to adjust it – to rotate it to see it better, the robot gets really small. And there is a warning that says : Exceeded the maximum number (8) of light sources

Leave a comment