We solved the kinematic model for the standard six-axes anthropomorphic robot in detail in Chapter 3 and Chapter 4. In this appendix, we briefly show the solutions for the other kinds of industrial robots presented in Mechanical Configurations Section in Chapter 1.
Please note that, in order to understand the techniques and notations used in this appendix, you need to be familiar with the content introduced when solving the standard six-axes robot. I suggest you get a good grasp of that solution’s details before tackling these other robots.
COBOT
Cobots (collaborative robots) are very popular these days because they are very simple to set up and deploy and especially because they are safe to deploy right beside human operators without being confined behind protection cages.
Their mechanical structure is a bit different than the standard six-axes manipulator that we analyzed in-depth in this book, and the kinematic model needs to be adjusted accordingly.
Figure
A-1 shows a Cobot
model in its home position and the names assigned to the mechanical parameters for each link. You can also choose a different home position and give different names to the parameters, but the concept does not change.
The first joint
J1 rotates around the vertical Z axis (Figure
A-2).
Then we move along the Z and Y
axes by
a1
z and
a1
y to reach the second joint
J2, which rotates around the Y axis (Figure
A-3).
We further move along the X and Y axes by
a2
x and
a2
y to find the third joint
J3, which also rotates around the Y axis (Figure
A-4).
Then again add
a3
x and
a3
y to reach the
fourth joint
J4, also rotating around Y (Figure
A-5).
The fifth joint
J5 rotates around Z and is offset by
a4
z from
J4 (Figure
A-6).
Finally, we add
a5
y to reach the last joint
J6, which
rotates around Y (Figure
A-7).
No fewer than four out of six joints rotate around the Y axis, while the other two rotate around the Z axis. There is no rotation around the X axis. Such a mechanical structure is quite different from the standard six-axes manipulator we are used to.
To solve the direct kinematics, we follow the usual procedure: we build the homogeneous matrix for each joint taking into account the rotation and translation caused by the corresponding mechanical link. See Chapter 2 for details.
For
J1, we simply need a rotation around Z.
For joints
J2,
J3, and
J4, we have rotations around Y and additional position offsets. Careful with the third joint: the value
a2
y is actually negative, because the link points backward, in opposite direction of the Y axis.
For
J5 we add a rotation around Z. Here also, make sure that
a4
z is negative because it points downward.
Finally,
J6 introduces one more rotation around Y.
To find out the
TCP position as seen from the base frame, we need to chain-multiply the six homogeneous matrices together:
If you expand all the calculations, you can verify that the [X, Y, Z] position coordinates of the
TCP are as follows:
Notice the compact notation s234 = sin (J2 + J3 + J4).
The total rotation of the
TCP with respect to the base frame is the product of all the joint rotation matrices:
By expanding the product, you should find the following
rotation matrix:
The TCP rotation matrix can be decomposed in either Euler angles or quaternions according to your needs, as explained in Chapter 2 and Chapter 5, respectively.
Notice how the mounting flange of the last joint is oriented along the Y axis, as opposed to facing the X axis for the standard manipulator. Keep that in mind when calculating offsets for the tool.
Solving the inverse kinematics can be done in different ways: some approaches are more geometric; others are more analytical. We present here one of the possible solutions, definitely a valid one, but not the only one.
This step is much harder than solving the direct transformations: it is not even guaranteed to have a solution, and when a solution exists, it is not necessarily unique. Actually, given a specific TCP pose, there are eight possible joints solutions (and many more if you consider all the ±2
π offsets for each joint).
Figure A-8 shows a Cobot in different joint configuration reaching the same TCP pose. In the first two cases, the difference depends on the position of the first joint: we call them LEFT and RIGHT. In the third case, the configuration of the second and third joint is different: we call it DOWN, as opposed to UP for the first two examples.
Let’s start by finding the value of the first joint. The orientation frame of the
TCP is given by the rotation matrix
RTCP = [
n,
o,
a], which can be quickly composed from the given Euler angles [A, B, C]. Recall that [
n,
o,
a] are the column vectors of the rotation matrix and represent the orientation of the frame axes at the TCP:
The column vector
o is oriented along the local Y axis and is facing out of the mounting flange (see Figure
A-9). If we move from the TCP backward along that direction by a distance of
a5
y, we find the wrist point:
Observe that
P5 is mechanically restricted to lie on a plane that never intersects the origin of the robot. It is easier to see that from the top, as shown in Figure
A-10.
We call
ρ the distance between
P5 and the base origin of the robot on the two-dimensional XY plane. We can state that no inverse solution exists for values of
ρ smaller than
a6
y, which is the sum of all the links along the Y axis (
a1
y +
a2
y +
a3
y), all taken with their correct signs (remember that
a2
y is usually negative).
In practice, that means that the robot cannot physically reach any TCP position which would force the wrist center point to be too close to the base origin. On the other hand, given a valid target position for
P5, we can quickly find the value of
J1.
There are actually two configurations that we can choose for the first joint, either with the arm LEFT or RIGHT, which are rotated at about 180 degrees from each other. Not exactly 180 degrees, because the plane of P5 does not pass through the origin, as opposed to the case of the standard six-axes manipulator.
Let’s consider the standard LEFT configuration first (Figure A-11 left). The value of J1 (in yellow) is the difference between α1 (in purple) and α2 (in green).
The angle
α1 is determined by the position of
P5:
The angle
α2 is constrained by
a6
y and
ρ:
As usual, while one could solve via the arcsine function directly, I suggest implementing your code with the arctangent function for better computational stability.
The final solution for
J1 in the left configuration is as follows:
In case the selected solution must be the RIGHT configuration,
J1 has to turn all the way around (see Figure
A-11 right).
If no forcing constraint is given by the operator, we can choose either one of the two solutions, typically the one closer to the current joint value at the start of the movement.
Given J1 we can now find J5. There are two different possible approaches that led to the same result: a geometric and an algebraic one. We choose the latter, because it is computationally more efficient. The idea is to extract the value of the joint from the robot’s rotation matrices.
Recall that the total
TCP rotation matrix is the product of the six rotation matrices of each joint:
Multiply both sides by the inverse rotation matrix of the first joint:
We already derived all the individual rotation matrices while solving the direct transformations. Using the properties that the inverse of a rotation matrix is simply its transpose (
R1−1 =
R1T) and recalling Equation (
A-13), we can quickly expand Equation (
A-21) into the following expression:
We only focused on the second row ignoring the rest of the matrix, because we spot a lonely
c5, which will lead us to finding
J5. The elements on both sides of the identity must be equal, including the central element on the second column:
Since we already know J1, we can immediately calculate J5.
The sign of J5 is not uniquely defined: there are two possible robot’s configurations, one with J5 positive and another one with J5 negative. Both solutions are technically valid, but we will choose only one later on, based on the values we find for the remaining joint angles.
We now move on to finding
J6. Let’s consider again the same matrix equivalence in Equation (
A-23): since now both
J1 and
J5 are known, we can extract
J6 from the elements on the first and third columns:
Given the sine and cosine of
J6, we immediately can find its value with the arctangent function:
There is always one unique solution for J6, as long as J5 is not zero. Otherwise, the denominator is 0 and the robot is in a singularity. Mathematically, it means that there are infinite valid solutions for J6. We can simply force it equal to any value we desire, typically the current value of the physical joint at the beginning of the movement.
The next step is to find
J2,
J3, and
J4, which requires quite a lengthy procedure. The trick is looking at the robot from the side and working on the projected
plane along the first joint, as shown in Figure
A-12. We observe that
J2,
J3, and
J4 form a kinematic chain with three rotational joints: we need to find the points
P2 and
P4 and from there solve the triangle.
Just keep in mind that there are two possible options, either UP or DOWN. We will solve both configurations.
Let’s start from
P2. Observe in Figure
A-13 that the first joint of the robot automatically fixes the position of point
P2, which lies on the rotation axis of
J2.
We use the
forward kinematics to solve for
P2:
To find
P4, we move on the other end of the robot (Figure
A-14).
P4 lies on the rotation axis of
J4, and its position and orientation are fixed by
J5 and
J6, both of which we already know.
Starting from the orientation of the
TCP and removing the rotations for
J5 and
J6, we find the orientation of the frame
at point
P4:
Then we can also calculate the position of
P4 starting from
P5 and move back along the Z axis of the local frame of
P4:
At this point, we have both
P2 and
P4, and we can start solving the kinematic chain between the two of them. We call their geometrical distance
P42. The projection of
P42 on the side plane along the first joint is as follows:
a7y = a2y + a3y (A-32)
Figure
A-15 shows a top view to clarify.
The small difference along the Y axis (a7y) is caused by a mechanical offset, the sum of a2y and a3y. Remember that a2y is usually negative.
In order to have a physically valid solution, we need to make sure that
λ can be reached using the two links
a2
x and
a3
x. Two conditions must be verified; otherwise, the inverse transformation function will fail:
Now that the three sides of the triangle [
a2
x,
a3
x,
λ] are all known, solving for
J2 and
J3 is a matter of basic trigonometry (see Figure
A-16).
We first calculate
α and then derive
β and
γ using the
law of cosines:
Following the right-hand rule for the angles, we obtain the following values for the joints in the default UP configuration:
For the DOWN configuration,
The only joint we are missing at this point is
J4. The solution is shown in Figure
A-17 and requires finding the difference in orientation between the X axes of the frames in
P2 and
P4. We call that difference
α14, and also note that
In other words, once we know
α14, we can complete the solution of the
inverse kinematics by applying the following:
The vector
x1 is simply the direction of the local X axis at point
P2, which is parallel to ground and only depends on the first joint:
The vector
x4 is the direction of the local X axis at point
P4, which we calculated earlier on in Equation (
A-29), when we derived the orientation of point
P4:
We also need the direction normal to the plane containing these two vectors. We call it
n, and it is simply given by the local Y axis at point
P2:
The angle between two vectors in a three-dimensional space, lying on the same plane, is given by the following equivalence:
The determinant of the [x1, x4, n] matrix gives the sine of the angle between x1 and x4, while the dot product (x1 ∙ x4) gives the cosine. We use the arctangent function to extract the correct value of the angle.
It is critical to normalize all the three vectors before carrying on the calculations.
Using Equation (A-43), we can finally find J4. Now we have the values for all the six joint angles, although remember that we end up with two different sets of solutions because of the two possible signs for J5 after Equation (A-24). Select the one forced by the operator or the one closest to the current configuration of the robot before starting the movement.
SCARA
The SCARA robot can be built in different mechanical configurations according to its application requirements. The most typical option comes with four joints (three rotational and one translational) and offers four degrees of freedom: the TCP can be positioned along the [X, Y, Z] axes and rotated around the C coordinate.
Figure
A-18 shows a SCARA model with four joint axes in its home
position. It is conventional to place the base frame of the robot at the beginning of the first link avoiding any initial vertical offset from the base column.
The last parameter a4z is actually a mechanical coupling between the last two joints. Since most industrial SCARA robots are built that way, we include the parameter directly in the default transformations. If your robot does not have such a coupling, you can safely set the a4z coefficient equal to 0.
The first two joints
J1 and
J2 rotate around the vertical Z axis and are entirely responsible for positioning the [X, Y] coordinates of the TCP (see Figure
A-19 and Figure
A-20).
The third joint
J3 translates the TCP along the vertical Z axis (see Figure
A-21).
Finally, the fourth joint
J4 introduces an additional rotation around the Z axis (see Figure
A-22).
Although the SCARA’s mechanical structure is simple enough to be quickly solved geometrically, we still adopt the common approach of composing all the homogeneous matrices for each joint to find the solution to the direct kinematics. See Chapter 2 for details.
The first joint
J1 introduces a rotation around Z.
The second
joint J2 also rotates around Z, after an offset along the X and Z axes.
The third
joint J3 is of translational type and acts along the vertical Z axis.
Finally, the last joint
J4 introduces an additional
rotation around Z. In most robots, this rotation is achieved by means of a screw-type mechanical shaft, which has the side effect of lifting and lowering the TCP while rotating. The additional vertical movement must be accounted for in the offset part of the homogeneous matrix. The coupling coefficient
a4
z specifies how much vertical displacement is caused by a complete rotation of
J4. Use 360 for degrees or 2
π if your input values are in radians.
To find out the
TCP position as seen from the base frame, we need to chain-multiply the four homogeneous matrices together:
Expanding the calculations leads to the following:
As usual, we adopted the compact notation s12 = sin (J1 + J2).
To find the rotation of the TCP, we could multiply together the rotation matrices and extract the only relevant Euler angle C. However, it is much faster to observe the
geometry of the problem on the horizontal [X, Y] plane (see Figure
A-23), and notice that the TCP rotation is built up by the series chain of the three rotational joints:
The inverse transformations are best solved geometrically. Given the position and orientation [X, Y, Z, C] of the TCP, we need to find the four joint angles.
We start with the first two joints
J1 and
J2 and observe that they are responsible for determining the position of the TCP on the [X, Y] plane (see Figure
A-24). We also notice that the solution is not unique: there are two possible options for the arm configuration, either LEFT or RIGHT, to reach the same TCP position.
We call
ρ the two-dimensional distance between the TCP and the base frame on the [X, Y] plane.
It is possible to find a solution to the
inverse transformations only if the two links
a1
x and
a2
x can reach the distance
ρ.
The angle between the base X axis and the vector
ρ is given by the following:
The two possible configurations of the arm are symmetric with respect to the vector ρ, forming two equal triangles with sides a1x, a2x, ρ. We call α the angle between a1x and ρ and β the angle between a1x and a2x (both angles are in green in Figure A-24).
Now we can express the values of the first two joints as functions of
α and
β. For the RIGHT pose, we have the following:
All we are missing at this point are the values for
α and
β. The angle
β can be quickly found using the law of cosines in the triangle
a1
x,
a2
x,
ρ.
As usual, I suggest using the arctangent for better numerical stability.
Once
J2 is known, we can derive the projections of the second link (
l and
h):
Using either Equation (A-61) or (A-63), we can calculate J1.
Solving the last two joints is much more straightforward. From Equation (
A-56), we immediately find
J4:
Finally,
J3 is the only joint that affects the vertical displacement of the TCP from the base frame, with the addition of the fixed mechanical offsets (
a1
z,
a2
z) and the (optional) coupling effect from
J4.
PALLETIZER
The palletizer is a
four-axes robotic arm typically deployed in loading/unloading applications, where large and heavy loads need to be transferred from conveyors to pallets. With only four joints, this robot can control four degrees of freedom of its TCP: three position coordinates [X, Y, Z] and one rotation angle C around the vertical Z axis. Figure
A-25 shows a palletizer model in its home position.
The striking characteristic of its
mechanical structure is that it is able to keep the load always parallel to ground regardless of the value of its joint axes. This feature is very helpful when handling packages, so that they can be picked or placed in any position in space without any need to electronically control their tilt. The TCP always stays horizontal to ground, thanks to a double parallelogram structure in the links of the robot. Figure
A-26 and Figure
A-27 show this feature when joints
J2 and
J3 are rotated.
The
first parallelogram also introduces a mechanical coupling between joints
J2 and
J3. Specifically, a rotation of
J2causes a rotation of the
J3 physical axis by the same angle in reverse direction, even when the motors in
J3 do not move. When solving the direct and inverse transformations, we will need to introduce a compensation for the effect of this mechanical coupling, as we learned in the related Sections in Chapters
3 and
4.
The second effect of the
mechanical parallelograms is to keep the TCP frame parallel to the base frame. The movement acts on the passive joint at the wrist point WP by introducing a rotation of −(
J2 +
J3), as shown in Figure
A-28. There is no motor acting on this joint, but we still need to keep track of its passive rotation in the transformations.
Finally, the first and last joints (
J1 and
J4) of the palletizer robot both rotate around the vertical Z axis (see Figure
A-29).
In particular, J4 does not have any effect on the position of the TCP. It only affects its rotation angle C.
Despite being an open-chain kinematic, the extra mechanical links make this robot a hybrid between serial and parallel models. Nevertheless, the transformations are typically solved using a standard serial kinematic model and then adding coupling coefficients between the joints.
The direct kinematics could be solved geometrically because of the simple structure of the palletizer. However, we keep following the same notation as for the other robots, and we present here all the homogeneous transformations for each axis.
The first joint
J1 introduces a
rotation around Z.
The second joint
J2 rotates
around the Y axis, after an offset along the X and Z axes.
The third joint
J3 also rotates around the Y
axis after a vertical offset along Z.
The mechanical links introduce a negative rotation around the Y axis centered at the wrist point. The magnitude of the passive rotation is the opposite of the sum of
J2 and
J3.
Finally, the last joint
J4 introduces an additional rotation around
Z. The translational offset between WP and TCP can be both along the X and Z directions, depending on the mechanical built. Keep in mind that
a4z is usually negative, translating toward ground.
To find out the
TCP position as seen from the base frame, we need to chain-multiply the five homogeneous matrices together:
Expanding the calculations leads to the following:
When feeding the
joint angle values to the direct transformation functions, remember to add the effect of the coupling between
J2 and
J3:
The rotation angle C can be extracted from the rotation matrix, or simply geometrically derived, by observing that only the first and last joint have any effect on it:
The
inverse transformations can be derived geometrically observing the robot from the side, projected on a plane along the first joint (see Figure
A-30). We start by deriving the position of the wrist point from the TCP coordinates [X, Y, Z]:
Then, we find all the
geometrical parameters we need in order to solve the triangle (
a2
z,
a3
z,
ρ).
The condition we need to impose, for the transformations to return a valid solution, is that the arm links
a2
z and
a3
x can physically stretch enough to reach the wrist point.
We can now find all the unknown angles
α,
β,
γ. The last two, using the law of cosines,
As usual, I suggest using the arctangent for better numerical stability.
At this point, we can quickly derive the values for
J2 and
J3:
Unlike most other robots, the palletizer does not allow different configurations of its joint to reach the same TCP pose. The values found in Equations (A-92) and (A-93) are unique.
The result of the inverse
transformations for
J2 and
J3 must be adjusted to compensate for the mechanical coupling:
Solving for
J1 is straightforward, as it is the angle formed by the two coordinates [X, Y] of the TCP:
Finally, from Equation (
A-81), we calculate the angle of the last joint:
DELTA
A Delta (or Tripod) robot is a parallel kinematic chain typically employed in applications with small workspace and high-speed requirements.
Figure
A-31 shows a Delta robot with three joint axes in its home
position. The three rotational joints
Ji (
i = 1..3) are located on the upper platform and directly actuate the upper arms
hi, which link points
Ai to
Bi. The lower arms
ki link points
Bi to
Ci and are built using parallelogram structures, which automatically keep the lower platform parallel to the upper one. Forcing the TCP to have a constant orientation makes the kinematic model much easier to solve.
Most of the delta robots found in the industry feature an additional fourth axis to add a rotation capability to the TCP around the vertical Z axis. This extra joint acts directly on the TCP and does not affect the rest of the kinematic model, so we did not show it in the figures. The motor rotating the TCP is rarely mounted directly on the lower platform because that would increase its weight and reduce the dynamic performance of the robot. The motor is usually placed on the upper platform and connected to the TCP by means of a central flexible link.
The position of the
TCP is often translated from the lower platform by an offset
t. We call the center of the lower platform the wrist point (WP) and derive its position from the TCP as follows:
The center of the upper platform is the origin of the robot’s base frame. The first joint
J1is oriented along the X axis, while the two other joints
J2 and
J3 are oriented, respectively, at 120 and 240 degrees. The three
axes thus form an equilateral triangle (see Figure
A-32). The distance between the origin and the joints centers (
Ai) is
r. Similarly, on the lower platform, the distance between the WP and the (passive) joints centers (
Ci) is
s.
A movement of the actuated joints causes a translational displacement of the lower platform along the positional [X, Y, Z] coordinates. See Figure
A-33 for some examples.
Solving the direct kinematics consists in finding the position of the
WP given the angles of the joint axes (see Figure
A-34). The procedure requires the following steps:
Calculate the position of the Bi points.
Calculate the position of the Pi points (offset from Bi by s along the joint planes).
Find WP as the intersection of the three spheres centered in Pi of radius k.
Adopting the usual compact notation
c1 = cos
J1, the
B1 point along the X axis has coordinates:
Similarly, we find the
B2 and
B3 points, which lie on the joint planes rotated at ±120 degrees around the Z axis for the
J1 plane:
The passive universal joints in
Bi allow rotations of the lower
ki links around an entire sphere. In other words, the position of the
Ci points will be on spherical surfaces centered on
Bi and of radius
k. Since the WP is offset by
s from
Ci along the three joint axes planes, we first offset the points
Bi by
s to find the points
Pi, and then we construct the three spheres centered in
Pi. The intersection of the three spheres will be the WP.
There exist various possible ways of solving the problem of intersecting three spheres, both algebraic and geometric. One option involves solving a system of three quadratic equations in [X, Y, Z]. Another approach uses a technique called trilateration, which we show here in detail.
Given three spheres of radius k centered in Pi, let us find their intersection point [X, Y, Z]. It can be shown that there are actually two intersection points, both lying on a line perpendicular to the plane containing the three center points. Mathematically, there are also cases in which the spheres only intersect in one single point or do not intersect at all, in case the centers are spaced too far apart from each other. However, the arms of the delta robot are designed specifically to avoid these complications. The lower arms ki are longer than the upper arms hi, so the spheres always intersect. The fact that they intersect in two points is also irrelevant, because we always choose the location for WP lower than the upper base platform.
Figure
A-35 shows the
plane containing the three center points. We define the two vectors
U =
P2 −
P1 and
V =
P3 −
P1. From simple vector properties, we can derive the
frame:
The projections of
V over the
and
axes are as follows:
It can be proven that the coordinates of the intersection point in the
frame are as follows:
Notice that there are two solutions for the z coordinate. We already mentioned before that the only physically relevant value for the WP is the one below the upper platform of the robot. Also, the expression under the square root must be positive in order to return two distinct real values. That is always the case with standard industrial robots, because they are built to avoid mechanical violations.
Finally, we need to transform the
WP coordinates back into the robot’s base frame:
Adding the last vertical offset will return the actual TCP position:
Solving the inverse transformations is somewhat simpler. The position of the wrist point is given, which fixes the lower platform in space. The upper platform is also fixed by default, so all we need to do is find the values of the joint angles Ji for which the upper arm and the lower arm connect with each other. Given the symmetry of the robot, the problem can be solved individually for each axis.
The upper arms can only rotate around their pivots A, forcing the points B to describe circles in space. The lower arms, on the other hand, can more freely swing around the universal joints in C, allowing the point B to describe spheres in space. The solution to the inverse kinematics for each joint is the intersection
between the upper circle and the lower sphere, as shown in Figure
A-36.
Actually, if we project the geometry of the problem onto a plane containing the joint axis (e.g., the XZ plane for
J1), the lower sphere degenerates into a circle, and the solution becomes the intersection between two circles, which is straightforward to find. Notice that out of the two possible intersections, we pick the one with the robot’s arm pointing outward, not inward (see Figure
A-37).
Let’s consider the XZ plane and solve for
J1:
The upper circle is centered in A = [r, 0] and has radius h.
The lower circle is centered in C′ = [WPX + s, WPZ] and has radius .
The resulting equations of the two circles are as follows:
The system can be quickly solved by extracting z from the first equation and plugging it into the second to find x. If two distinct real solutions exist, we choose the one with the largest
x coordinate to keep the robot’s arm on the outside of the platform. The unique result is point
B = [
BX,
BZ], which forces the joint angle to be as follows:
If no real solution to the system is found, then the given TCP position is outside of the reachable robot’s workspace, and the inverse transformation function must return an error.
To find J2 and J3, we can simply follow the same procedure used for J1, but each time the XZ plane needs to be rotated by 120 degrees around the vertical Z axis. The equation of the upper circle does not change. However, the coordinates of the wrist point used in the lower circle need to be adjusted to the new frames.
For
J2, use a rotation of 120 degrees from
J1 around Z:
For
J3 use a rotation of −120 degrees from
J1 around Z:
We can plug these values in Equation (A-114) and find the joint angles from Equation (A-115).