We already know how to describe the movement of a rigid body separately.

Our robot is formed by the junction of rigid bodies (the links and the end-effector). But they do not move independently! It is the movement of the joints of the robot that provides this movement.

Relating the joint movement to the movement of the links/end-effector is the goal of Forward Kinematics.

We start with an important concept/definition: configuration.

In the context of fixed-base, rigid-link, open-chain manipulators, the configuration of the robot at a given instant of time, \(q\), is nothing more than a column vector with \(n\) elements, where \(n\) is the number of joints, that has the rotation (rotary joint)/ translation (linear joint) of the joint at that instant of time.

The direction of rotation/translation of the joint when the value of \(q_i\) increases and the rotation/translation corresponding to the zero value of the joint (\(q_i=0\)) is a purely conventional matter.

This is common in physics (when creating a reference frame, you define what the zero point is, what the positive direction of the \(x\) axis is, etc...).

Usually, this is already defined in the robot, but it can be easily changed if it is convenient for you.

See the various configurations on the side, where it is possible to determine the chosen direction of rotation (counterclockwise) and the zero rotation for the two joints.

Note that, in possession of the configuration, we can determine the position of any particle of the robot.

The general definition of configuration in Mechanics is this: the information necessary to, at that instant of time, determine the position of any particle of a system.

In our case, this information is the rotation/translation of the rotary and linear joints, respectively.

Let's calculate a first example of forward kinematics. Consider the planar robot on the side, with two rotary joints.

Both links are 0.5m.

Calculate \(T^{e}_0(q)\), that is, the HTM of the reference frame attached to the end-effector, \(\mathcal{F}_e\) relative to the reference frame \(\mathcal{F}_0\), as a function of the configuration \(q\).

Consider that the positive direction of rotation of the two joints is counterclockwise. Also, that in \(q_1=q_2=0\), the robot is fully stretched to the right.

Let's solve the problem in parts. First, we will calculate the vector \(s(q)\), which is the center of the reference frame \(\mathcal{F}_e\) written in the reference frame \(\mathcal{F}_0\).

We start by drawing the vector \(v_1\) that goes from the center of \(\mathcal{F}_0\), which is the center of the first joint, to the center of the second joint...

With basic trigonometry, we find that the \(x_0\) component of this vector is \(0.5\cos(q_1)\), and the \(z_0\) component is \(0.5\sin(q_1)\). The \(y_0\) component is \(0\).

So: $$v_1(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0.5\cos(q_1) \\ 0 \\ 0.5\sin(q_1) \end{array}\Large{\Bigg)}\normalsize{}.$$

Now let's draw the vector \(v_2\), which goes from the center of the second joint to the center of \(\mathcal{F}_e\).

With a bit of geometry, we find that the angle this vector makes with the \(x_0\) axis is \(q_1+q_2\).

Again, with basic trigonometry, we find that the \(x_0\) component of this vector is \(0.5\cos(q_1+q_2)\), and the \(z_0\) component is \(0.5\sin(q_1+q_2)\). The \(y_0\) component is \(0\).

So: $$v_2(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0.5\cos(q_1+q_2) \\ 0 \\ 0.5\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

Therefore, the vector \(s(q)\) is \(v_1(q)+v_2(q)\):

So: $$s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0.5\cos(q_1)+0.5\cos(q_1+q_2) \\ 0 \\ 0.5\sin(q_1)+0.5\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

We now need to find the rotation matrix \(Q(q)\).

Note, from the image on the side, that, disregarding the translation (already resolved when we calculated \(s(q)\)), the reference frame \(\mathcal{F}_e\) is the reference frame \(\mathcal{F}_0\) rotated by \(q_1+q_2\) on the \(y_0\) axis in the clockwise direction! Since we defined the positive rotation as being in the counterclockwise direction, it will be a rotation of \(-(q_1+q_2\)) in \(y_0\).

Therefore:

$$ Q(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(q_1+q_2) & 0 & -\sin(q_1+q_2) \\ 0 & 1 & 0 \\ \sin(q_1+q_2) & 0 & \cos(q_1+q_2) \end{array}\LARGE{\Bigg)}\normalsize{}$$

in which we used the fact that \(\cos(-\theta)=\cos(\theta)\) and \(\sin(-\theta)=-\sin(\theta)\).

Finally, using the notations \(c_1=\cos(q_1)\), \(s_1=\sin(q_1)\), \(c_{12}=\cos(q_1+q_2)\) and \(s_{12}=\sin(q_1+q_2)\):

in which \(q=(q_1 \ q_2)^T\).

The next example is similar to the first, but we have a linear joint before the end-effector.

Two rotary joints and one linear joint.

The third joint has the positive direction pointed by \(x_e\). The value \(q_3=0\) corresponds to a link of \(0.25m\) (see the image on the side).

The solution is very similar to the previous one.

The difference is the vector \(v_2\). In Example 1, we used the link length as \(0.5m\). Now we will use \(0.25+q_3\). The vector will then be:

So: $$v_2(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} (0.25+q_3)\cos(q_1+q_2) \\ 0 \\ (0.25+q_3)\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

The final HTM will be:

$$ H_0^e(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{cccc} c_{12} & 0 & -s_{12} & 0.5c_1+(0.25+q_3)c_{12} \\ 0 & 1 & 0 & 0 \\ s_{12} & 0 & c_{12} & 0.5s_1+(0.25+q_3)s_{12} \\ 0 & 0 & 0 & 1 \end{array}\LARGE{\Bigg)}\normalsize{}$$

in which \(q=(q_1 \ q_2 \ q_3)^T\).

Note that, in the two previous examples, there are combinations of configuration \(q\) that would be invalid.

For example, in a certain configuration, there may be a collision between links.

Or even, in the case of the linear joint in Example 2, there are clearly limits \(q_{3,min} \leq q_3 \leq q_{3,max}\), because the joint cannot extend indefinitely in one direction or the other.

Rotary joints can also have factory limits, created to avoid collisions.

In fact, for each robot, there is a subset of \(q\) values that are prohibited, due to these factors.

In this course, we will essentially ignore this. We will assume that our controllers do not guide the configuration to these prohibited values.

This is reasonable, actually, because robots are built to minimize this problem and the tasks we deal with in the industrial environment are not extremely complex.

There are techniques to explicitly consider this problem in the controller, but they are beyond the scope of this course.

For planar robots, where it is not necessary to worry about 3D movements, we can use analytical geometry techniques to calculate the forward kinematics for the end-effector.

But for general robots, this is not very practical, and the analysis would become very ad-hoc .

To standardize the process of calculating forward kinematics, the Denavit-Hartenberg (DH) convention was created.

The idea is simple: the DH convention tells you how to attach reference frames to each of the rigid bodies of the robot.

With this convention, there is a systematic and simple way to calculate the HTM from one reference frame to the next, until we have the HTM from the reference frame \(\mathcal{F}_0\) to the end-effector reference frame, \(\mathcal{F}_e\).

For a robot with \(n\) joints, the DH convention creates \(n+1\) reference frames.

We will name these reference frames \(\mathcal{F}_{DHi}\), \(i=0,1,...,n\).

There is a "recipe" to create these reference frames. Do the following for \(i=0,...,n\), where the joints are numbered from \(1\) to \(n\)

The reference frame \(\mathcal{F}_{DHi}\) is attached to the i-th link, with link 0 being the base of the robot (immobile).

There are some cases where the rule is ill-defined, and the engineer has the freedom to choose:

• For \(i=0\), there is no joint₀. So, for \(i=0\), simply choose the center arbitrarily on the line₁, and the \(x_{DH0}\) axis arbitrarily, as long as it is orthogonal to the \(z_{DH0}\) axis.
• If the lines line_i and line_i+1 are parallel, there are infinite possible choices for the pair of points point_i,i+1 and point_i+1,i. Choose one of these pairs arbitrarily.
• If point_i,i+1 =point_i+1,i, the rule for choosing \(x_{DHi}\) is ill-defined. Choose any of the two vectors that are mutually orthogonal to the lines line_i and line_i+1.
• If \(i=n\), there is no joint_n+1. In this case, choose the line line_n+1 arbitrarily. Generally, we use the previous line, line_n.

After attaching \(n+1\) reference frames to the robot (one of them on the fixed base), there is a systematic way to calculate the HTM \(H_{DH0}^{DHn}(q)\), which is the last DH reference frame (\(i=n\)) written in the first DH reference frame (\(i=0\)).

Given the two interpretations for an HTM, we can also interpret \(H_{DH0}^{DHn}(q)\) as the rigid transformation that we have to make from the first reference frame to the last.

In any case, for this, we need to extract the Denavit-Hartenberg Parameters of the robot, which are \(4n\) numbers that allow us to know how each of the reference frames is at a given instant of time.

There are 4 parameters to transform from the reference frame \(\mathcal{F}_{DH(i-1)}\) to the next, the reference frame \(\mathcal{F}_{DHi}\). This is for \(i=1,...,n\), totaling \(4n\) parameters.

These are the 4 parameters:

The way the DH axes are attached ensures the existence of these parameters, that is, there are numbers \(\theta_i,d_i,\alpha_i,a_i\) such that these transformations parameterized by them transform one reference frame into the next.

All three remaining parameters from one reference frame to the next are structural constants of the robot, decided at the time of its conception.

Note that the transformations always use the current reference frame, not the initial reference frame...

Note that in the two previous examples, the robot was the same, but in different configurations.

The structural parameters (in white) were the same, but the variable ones (in yellow) changed.

In this case, since all the joints were rotary, all the \(\theta_i\) are variable, depending on the configuration \(q_i\).

In possession of the parameters, it is very easy to calculate the transformation from the reference frame \(\mathcal{F}_{DH(i-1)}\) to the \(\mathcal{F}_{DHi}\), for \(i=1,2,...,n\).

The transformation is as follows: $$H_{DH(i-1)}^{DHi}(q_i) = R_z(\theta_i)D_z(d_i)R_x(\alpha_i)D_x(a_i).$$

Note that the transformation should be read from left to right, as we use the current reference frame, not the initial one, to interpret the transformations!

So first is the rotation in \(z\), then the displacement in \(z\), then the rotation in \(x\), and finally the displacement in \(x\).

The HTM \(H_{DH(i-1)}^{DHi}(q_i)\) can be interpreted either as the transformation from \(\mathcal{F}_{DH(i-1)}\) to \(\mathcal{F}_{DHi}\) interpreted in \(\mathcal{F}_{DH(i-1)}\) (interpretation as a rigid transformation) or as the reference frame \(\mathcal{F}_{DHi}\) written in \(\mathcal{F}_{DH(i-1)}\) (interpretation as a reference frame).

Note that \(H_{DH(i-1)}^{DHi}(q_i)\) depends on \(q_i\), because either \(\theta_i\) (rotary joint) or \(d_i\) (linear joint) depends on \(q_i\).

The HTM \(H_{DH0}^{DHn}(q)\) between the first reference frame (fixed) and the one attached to the last link (end-effector) is: $$H_{DH0}^{DHn}(q) = H_{DH0}^{DH1}(q_1)H_{DH1}^{DH2}(q_2)...H_{DH(n-1)}^{DHn}(q_n).$$

It depends on the entire configuration \(q\).

Not always the first reference frame created by DH, \(\mathcal{F}_{DH0}\), is the reference frame you wanted to be "the" reference frame \(\mathcal{F}_0\).

Similarly, not always the last DH reference frame, \(\mathcal{F}_{DHn}\), is the reference frame you would like to be the end-effector reference frame \(\mathcal{F}_e\).

Fortunately, in both cases, there will be constant (independent of \(q\)) transformations, \(H_{0}^{DH0}\) and \(H_{DHn}^{e}\), respectively, that allow us to make this adjustment.

The DH convention, although it has not completely solved the problem in these cases, has solved the most complex part of the problem, which is finding the part of the transformation that is variable, depending on \(q\).

With these two adjustment matrices, we have: $$H_{0}^e(q) = H_0^{DH0} H_{DH0}^{DHn}(q) H_{DHn}^{e}.$$

The DH parameters can be obtained by measuring the robot directly, or from its manual.

Unfortunately, there are other conventions besides DH. So, sometimes the manual provides the parameters for another convention.

In any case, it is the most used definition.

In the problem of inverse kinematics, we have a desired HTM for the end-effector, \(\hat{H}_{0}^{e}\) and we want to find \(q\) such that \(H_0^{e}(q) =\hat{H}_{0}^{e}\).

This problem can have one, none, or multiple solutions.

We have to solve a complex system of nonlinear equations.

To illustrate this complexity, let's solve a problem that is, apparently, very simple.

Consider the planar robot with two rotary joints on the side. We want the position of the end-effector to be a desired position \(x=x_a\) and \(z=z_a\).

We do not care about the orientation. It is a "partial" inverse kinematics problem.

Even so, we will see that it is not simple.

Using the notation of \(c_1,s_1,c_{12}\) and \(s_{12}\) defined earlier, we have the equalities:

$$L_1c_1 + L_2c_{12} = x_a,$$

$$L_1s_1 + L_2s_{12} = z_a$$

obtained by forward kinematics (see Example 1 of Forward Kinematics).

Square both equalities and add them.

Use the fact that \(c_1^2+s_1^2 = c_{12}^2+s_{12}^2 = 1\), and that \(c_1c_{12} + s_1s_{12} = \cos(q_2)\):

$$L_1^2+L_2^2 + 2L_1L_2\cos(q_2) = x_a^2+z_a^2.$$

And therefore:

$$\cos(q_2) = \frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}.$$

For the equation

$$\cos(q_2) = \frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}$$

to have a solution, the right-hand side must be between -1 and 1. Rewriting this condition, we have:

$$(L_1-L_2)^2 \leq x_a^2+z_a^2 \leq (L_1+L_2)^2$$

Reflect a little and you will see that this condition makes perfect sense, geometrically.

If the condition is satisfied, we have two solutions:

$$q_2 = \pm \cos^{-1}\left(\frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}\right).$$

Let's assume from now on that we choose one of these two solutions. Substituting into the original system and using \(c_{12} = c_1c_2-s_1s_2\) and \(s_{12} = c_1s_2+c_2s_1\) (where \(c_2=\cos(q_2)\) and \(s_2=\sin(q_2)\)):

$$(L_1+L_2c_2)c_1 - L_2s_2s_1 = x_a,$$

$$L_2s_2 c_1 + (L_1+L_2c_2)s_1 = z_a$$

We then have a linear system with two equations and two unknowns for the variables \(s_1\) and \(c_1\):

$$\left(\begin{array}{cc} L_1+L_2c_2 & -L_2s_2 \\ L_2s_2 & L_1+L_2c_2\end{array}\right)\left(\begin{array}{c} c_1 \\ s_1 \end{array}\right) = \left(\begin{array}{c} x_a \\ z_a \end{array}\right).$$

Isolating:

$$c_1 = U = \frac{(L_1+L_2c_2)x_a + L_2s_2z_a}{G}$$

$$s_1 = V = \frac{-L_2s_2x_a + (L_1+L_2c_2)z_a}{G}$$

where \(G = L_1^2+L_2^2 + 2L_1L_2c_2\).

Using the expression for \(c_2\) that we have, it is easy to see that \(G=x_a^2+z_a^2\). Therefore, if we assume that the target point is not at the origin, we have that \(G \not=0\).

For a system of equations of the form \(\cos(q_1)=U\) and \(\sin(q_1)=V\) to have a solution, it is necessary and sufficient that \(U^2+V^2=1\). In fact, we can show that this is true in this case.

In this case, the solution for \(q_1\) is unique. Generally, in computational systems, the function atan2(V,U) gives this value.

We have the Python code to calculate the inverse kinematics:

import numpy as np

def find_q1(L1,L2,xa,za,q2):
  G =  xa**2+za**2
  c2 = np.cos(q2)
  s2 = np.sin(q2)
  U = ( (L1+L2*c2)*xa + L2*s2*za )/G
  V = ( -L2*s2*xa + (L1+L2*c2)*za )/G
  return np.arctan2(V,U)

def solve_ik(L1,L2,xa,za):
  G =  xa**2+za**2
  if G >= (L1+L2)**2 or G <= (L1-L2)**2:
    raise Exception("No solution!")
  else:

    q2_A = np.arccos( (G - (L1**2+L2**2))/(2*L1*L2) )
    q2_B = -q2_A

    return [find_q1(L1,L2,xa,za,q2_A), q2_A], [find_q1(L1,L2,xa,za,q2_B), q2_B]


#Test:
L1 = 1
L2 = 1.5
xa = 0.8
za = 0.5

q_A, q_B = solve_ik(L1,L2,xa,za)

print(q_A)
print(q_B)
                        

Considering \(G \not=0\), when we have a solution, we have two solutions (see the side).

The exception is when \(q_2=0\), where we only have one solution.

The case where \(G=0\) can be treated separately.

Solving the inverse kinematics problem for a three-dimensional robot with several degrees of freedom analytically, as we did here, is often unfeasible

There is research dedicated solely to the problem of analytical inverse kinematics for specific robots.

However, numerical methods are more practical for general robots.

The technique of kinematic control that we will see soon can be easily adapted for this purpose.

In UAIBot, we can access the current configuration of the robot as robot.q, where robot is the name of the robot variable:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Show the current configuration
print(robot.q)
#Show the "base" configuration
print(robot.q0)
                        

Both are numpy vectors with shape (n,1) (n rows and 1 column).

We can specify the configuration at time \(t\) with .add_ani_frame.

When it is called, we change the current configuration of the robot (robot.q).

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])

#Let's make the first joint vary from 0 to 2*pi, while the others remain at
# zero. The time will vary from 0.01 to 0.01 seconds, up to 10 seconds.
dt=0.01
tmax=10
for i in range(round(tmax/dt)):
    t = i*dt
    robot.add_ani_frame(time = t, q = [ (2*np.pi)*t/tmax, 0, 0, 0, 0, 0])

#Let's see the result
sim.run()
                        

Forward kinematics can be calculated with the function .fkm (from Forward KineMatics).

All HTMs are calculated with respect to the simulation scenario reference frame:

import numpy as np
import uaibot as ub

#Just configure how the matrices will be displayed
np.set_printoptions(precision=4, suppress=True)

robot = ub.Robot.create_kuka_kr5()

#Without parameters, calculate for the current configuration (robot.q) and the end-effector
print(robot.fkm())

#We can specify the configuration
print(robot.fkm(q=[0,1,2,3,4,5]))

#We can choose whether we want to calculate the HTM for the end-effector (default) or for
#the DH reference frames. In the latter case, we have a list with n matrices
#(for the first reference frame, FDH0, it is not calculated, as it is fixed)

#Calculate for the end-effector
print(robot.fkm(q=robot.q, axis='eef'))
#Calculate for the DH reference frames
htm_dh = robot.fkm(q=[0,1,2,3,4,5], axis='dh')
for i in range(len(htm_dh)):
    print(htm_dh[i])
                        

In UAIBot, we can access the DH parameters of the robot:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()

#The joints go from i=0 to i=n-1
for i in range(len(robot.links)):
    #Type: 0 (rotary), 1 (linear)
    print("Type"+str(i+1)+": "+str(robot.links[i].joint_type))
    print("theta"+str(i+1)+": "+str(robot.links[i].theta)+" rad")
    print("d"+str(i+1)+": "+str(robot.links[i].d)+" m")
    print("alpha"+str(i+1)+": "+str(robot.links[i].alpha)+" rad")
    print("a"+str(i+1)+": "+str(robot.links[i].a)+" m")
    print("\n")


                        

Inverse kinematics can be calculated with the function .ikm (from Inverse KineMatics):

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])

#Let's create a target HTM for the end-effector that is equal to the initial one, but shifted
#a little down
target =  ub.Utils.trn([0,0,-0.3]) * robot.fkm()

#Let's find out what the configuration is that gives this HTM for the end-effector:
q_ikm = robot.ikm(htm_target = target)

#Let's make a smooth animation of the robot going from the initial configuration to
#the target configuration
dt=0.01
tmax = 10
for i in range(round(tmax/dt)):
    t = i*dt
    q_c = (1-t/tmax) * robot.q0 + (t/tmax) * q_ikm
    robot.add_ani_frame(time=t, q=q_c)

sim.run()