This is a geometric perspective into the inverse kinematics for Universal Robots UR5 and UR10. There is something very interesting about the kinematics model of these little robots, which is rather different than our KUKA and ABB systems. It is possible to solve the inverse kinematics with pure constructive straight-edge and compass geometry.
Assumptions
For consistency I will use a right-handed coordinate system convention throughout the post whereby colour coding wise: x-axis → red, y-axis → green and z-axis → blue. In addition, positive rotation at the joints is assumed counter clock wise about the z-axis of the local coordinate frames. The directions of xy-plane vectors are quasi-arbitrary but they have been chosen carefully to match the readouts of the pendant and rotation directions of the actual robot.
Kinematic Model
For simplicity we abstract the robot to a kinematic skeleton of its axes assuming, without any particular loss of generality, that the base point is situated at the world origin: ( 0, 0, 0 ) with standard coordinate directions x-axis: ( 1, 0, 0 ), y: ( 0, 1, 0 ) and z: ( 0, 0, 1 ), as shown below. The robot has no tool end effector attached, therefore the target is assumed at the front face centre point of the flange.
Home & Zero Positions
The zero joint angle position J_{1..6} = ( 0, 0, 0, 0, 0, 0 ) → ( origin: ( -1184.300, -256.141, 11.600 ), x-axis: ( 1, 0, 0 ), y-axis: ( 0, 0, 1 ), z-axis: ( 0, -1, 0 ) ) for UR seen below as the horizontal pose is different than its home position which is J_{1..6} = ( 0, -90, 0, -90, 0, 0 ) → ( origin: ( 0.000, -256.141, 1427.300 ), x-axis: ( -1, 0, 0 ), y-axis: ( 0, -1, 0 ), z-axis: ( 0, -1, 0 ) ) seen below as the upright pose.
Nomenclature
J0 is assumed the world origin which coincides with the robot’s base coordinate system. J7 is assumed the target centre point. There is no rotation applied to these coordinate systems so they behave as pseudo-joints. In the diagram below the intermediate points between axes are also marked serially as A..G.
Axial Offsets
In order to fully define the geometry of the robot apart from the axes angle configuration we need a table of axial offsets from one joint to the next. The table below contains the relative and absolute offsets for our UR10 in millimetres:
Relative | Delta X | Delta Y | Delta Z | ||
J0 | .. | J1 | 0.000 | 0.000 | 38.000 |
J1 | .. | J2 | 0.000 | -86.000 | 89.300 |
J2 | .. | J3 | -612.000 | -30.300 | 0.000 |
J3 | .. | J4 | -572.300 | 6.859 | 0.000 |
J4 | .. | J5 | 0.000 | -54.500 | -61.700 |
J5 | .. | J6 | 0.000 | -61.400 | -54.000 |
J6 | .. | J7 | 0.000 | -30.800 | 0.000 |
Geometric Observations
The rotation axes J2,3,4, highlighted below, are parallel in any configuration. The projections of J2,3,4 origins onto J1zx (or J2xy) plane form a triangular frame with fixed lengths BC and DE which we will use to determine the angle of J3 once we determine the projected distance from J2 to J4.
It is also important to note that the three consecutive parallel axes J2,3,4 introduce a constraint on the position of the origin of J5 (and points G and F) in relationship to plane J1zx (or J2xy). In paricular the projection distance of those points onto J1zx plane is fixed to 163.941mm (see table above). Below point X is shown as the projection of G onto J1zx. We will use this to determine the orientation of J1y/J2z.
In addition, we observe that the positions of points B/C and D/E, are irrelevant to the notional kinematic model of the robot. For instance we could offset B/C and D/E by a factor along J1y without changing anything. They are however important in a physical sense in terms of how the robot folds together, creating potential for self-collision for instance.
The Inverse Kinematics Problem
Quite simply stated, given the position and orientation of J7, find all possible configurations of the joint angles that match said pose. In fact there may as a many as eight different joint angle vectors that satisfy a target basis as seen below. Those are results of three bits of symmetry that emerge from geometry.
There are two ways to look at locating the positions of the intermediate joints between J0..7. We could start from the base and move upwards or the other way around. In both scenarios it is quite evident that if we have one additional hint about either the orientation of J2z or J5z the problem becomes trivial.
Top-Down Solution
We start from the origin of J7 and locate point G (where J5z and J7 cross) by moving back from J7 by 92.2mm (axial offset Jy5..7). We draw a cylinder at J0xy along J0z with radius 163.941mm (axial offset for Jy0..5) and find its intersection with the plane P with origin at G and normal J7z. If the intersection curve K exists then it will be an ellipse in general or a circle in case J7z is parallel to J0z. We draw the tangent lines from G to K. The directions from the centre of K to the tangent points T1 and T2, projected on J0xy define the two solutions for J2z. We then intersect the tangent lines of K with a new circle on plane P with centre G and radius 115.7mm (axial offset Jz4..5). The directions from G to the intersections points Wa1,2 & Wb1,2 define the solutions for J5z.
If there is no intersection (the plane intersects the cylinder at infinity if you will) it is because J0z is perpendicular to J7z which implies that J0z is parallel to J5z, thus J5z is ±J0z. Finally J1z can be produced by the intersection of the two vertical tangent planes from G onto the cylinder and the horizontal plane J0xy. The bottom-up solution explain the reasoning in detail.
Bottom-Up Solution
First we determine the direction of J2z. Looking back at the constraint of axes J2z, J3z and J4z being parallel we infer that the absolute axial offeset between J0 and J5, 163.941mm, defines a circle with centre at the origin of J0 where the tangents lines from G (where J5z and J6z cross) to the circle, in projection onto the J0xy plane, determine the directions of the links and the perpendicular to it defines the two possible directions of J2z.
The first symmetry bit comes from the possibility of the robot’s elbow being oriented on the left or right side of the line connecting its point at the origin and point G. It is easier to understand from the plane J0xy view below. It is also interesting to note that if the point G is within the circle then there is no solution.
If we know the direction of J2z then we also know J3z and J4z. Moving back 92.2mm along J7z from J7 we locate the point G (where J5z and J7 cross). J5z is the radial direction of a circle with centre at G and radius 115.7mm (the numbers come from the table of axial offsets). The circle represent all possible positions for point F (where J4z and J5z cross) as seen the previous diagram. But we know that the frames J4,5 and J5,6 are square, therefore the direction of J5z must be perpendicular to both J4z and J7z, or in other words J4z is tangent to this circle. We thus find the direction of J5z and its inverse which define one symmetry branch in the tree of solutions.
The position of J3 is also very easy to derive given the positions of J2 and J4. Projecting the origin of J4 on the plane J2xy we are in a circumstance where we know three lengths, and two points, of the triangle J234 (J23 and J34 from the axial offsets and J24 from the derived points) but not the angles or the last point J3. The position of J3 can be on either of the intersections of the two circles with centres at J2 and J4 with radii J23 and J34, respectively. If the target J7 is far enough from J0 such that J23 + J34 < J24 then there will be no solution because the robot is over stretched. There is another corner case where the internal angle at J3 is zero (or about) but then we have physical self collision issue rather than kinematic.
Algebraic Solution
The following technical paper offers an excellent algebraic solution to the inverse kinematics problem of the Universal Robot using transformation matrices.