Solving the Forward Kinematics of a Stanford Arm
This is my implementation of the forward kinematics problem in Robotics. The forward kinematics problem involves finding the end-tip position of a manipulator in a coordinate space given its joint parameters (i.e., joint angles for revolute joints and link offset for prismatic joints). This means that given a certain pose of a robotic arm, the xyz coordinates of the hand must be determined. For this implementation, I will be using the 6-DOF Stanford Manipulator as my basis.
Stanford Arm
The Stanford Arm, designed by Victor Scheinmann in 1969, can be considered to be one of the classic manipulators in robotics, and is one of the first robots that are designed exclusively for computer control. In this project, I performed a forward kinematics procedure in simulating the arm.
Forward Kinematics uses different kinematic equations in order to compute for the end-tip position of a manipulator given its joint parameters. Joint parameters can refer to joint angles \(\theta\) for revolute joints, or link lengths for prismatic joints. In solving for the Forward Kinematics, I utilized the Denavit-Hartenberg (DH) Parameters.
Methodology
Denavit Hartenberg Parameters
With DH Parameters, solving for the Forward Kinematics is easy. I only need to take four parameters for each joint \(i\): \(\theta_{i}\) for the joint angle, \(\alpha_{i}\) for the link twist, \(d_{i}\) for the link offset, and \(a_{i}\) for the link length. Once I’ve obtained them, I can just plug them in to this transformation matrix:
\[T_{i-1}^{i} = \begin{bmatrix} \cos(\theta_{i}) & -cos(\alpha_{i})\cdot sin(\theta_{i}) & \sin(\alpha_{i})\cdot\sin(\theta_{i}) & a_{i} \cdot cos(\theta_{i}) \\ \sin(\theta_{i}) & cos(\alpha_{i}) \cdot cos(\theta_{i}) & -sin(\alpha_{i}) \cdot cos(\theta_{i}) & a_{i} \cdot sin(\theta_{i}) \\ 0 & sin(\alpha_{i}) & cos(\alpha_{i}) & d_{i} \\ 0 & 0 & 0 & 1 \end{bmatrix}\]For the Stanford Arm, here are the DH Parameters:
| Joint | Joint Angle | Link Offset | Link Length | Link Twist |
|---|---|---|---|---|
| 1 | \(\theta_{1}\) | \(d_{1}\) | 0 | -90 |
| 2 | \(\theta_{2}\) | \(d_{2}\) | 0 | -90 |
| 3 | -90 | \(d_{3}\) | 0 | 0 |
| 4 | \(\theta_{4}\) | \(d_{4}\) | 0 | -90 |
| 5 | \(\theta_{5}\) | 0 | 0 | 90 |
| 6 | \(\theta_{6}\) | \(d_{6}\) | 0 | 0 |
Table 1: DH Parameters for the Stanford Arm
The joint variables are then expressed as a vector \(V\):
\[V \equiv \begin{bmatrix} \theta_{1} & \theta_{2} & d_{3} & \theta_{5} & \theta_{6} \end{bmatrix}\]They are then limited in terms of physical constraints, such that:
| Parameters | Limitation |
|---|---|
| \(\theta_1\) | [-180 180] |
| \(\theta_2\) | [-90 90] |
| \(d_3\) | [1 3] |
| \(\theta_4\) | [-180 180] |
| \(\theta_5\) | [-25 25] |
| \(\theta_6\) | [-180 180] |
Table 2: Joint variable physical constraints
MATLAB Implementation
The following code snippets will show my implementation of the forward kinematics of the Stanford Manipulator in MATLAB.
Transformation Matrix
The transformation matrix is expressed as the function getTransformMatrix.
It takes the DH parameters of a single joint as its input argument, and
outputs the corresponding transformation matrix \(T_{i-1}^{i}\).
function [T] = getTransformMatrix(theta, d, a, alpha)
T = [cosd(theta) -sind(theta) * cosd(alpha) sind(theta) * sind(alpha) a * cosd(theta);...
sind(theta) cosd(theta) * cosd(alpha) -cosd(theta) * sind(alpha) a * sind(theta);...
0,sind(alpha),cosd(alpha),d;...
0,0,0,1];
end
Forward Kinematics
Forward kinematics then becomes a simple implementation of the
getTransformMatrix above. By setting the link lengths constant, the end-tip
position can be computed. In my case, I wrapped this function inside the
method forwardKinematics:
function [T00,T01,T12,T23,T34,T45,T56,Etip] = forwardKinematics(theta1,theta2,d3,theta4,theta5,theta6)
T00 = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
T01 = getTransformMatrix(theta1,d1,0,-90);
T12 = getTransformMatrix(theta2,d2,0,90);
T23 = getTransformMatrix(0,d3,0,-90);
T34 = getTransformMatrix(theta4,d4,0,-90);
T45 = getTransformMatrix(theta5,0,0,90);
T56 = getTransformMatrix(theta6,d6,0,0);
Etip = T00 * T01 * T12 * T23 * T34 * T45 * T56;
end
Simulation Results
Here are some simulations of my work. I tried different values for the joint parameters and then recorded the results in GIF format. I also plotted the top and side views just to see how the manipulator is posed in terms of the x-y and x-z coordinates.
Simulation 1
\(\theta_{1}\) = -90, \(\theta_{2}\) = 90, \(d_{3}\) = 6, \(\theta_{4}\) = 45, \(\theta_{5}\) = 25, \(\theta_{6}\) = 90
Figure 1a: Forward Kinematics Simulation
Figure 1b: Top (left) and side (right) views of the manipulator’s end-pose
Simulation 2
\(\theta_{1}\) = 180, \(\theta_{2}\) = 0, \(d_{3}\) = 6, \(\theta_{4}\) = 45, \(\theta_{5}\) = 25, \(\theta_{6}\) = 0
Figure 2a: Forward Kinematics Simulation
Figure 2b: Top (left) and side (right) views of the manipulator’s end-pose
Simulation 3
\(\theta_{1}\) = 90, \(\theta_{2}\) = -45, \(d_{3}\) = 6, \(\theta_{4}\) = 45, \(\theta_{5}\) = -25, \(\theta_{6}\) = 45
Figure 3a: Forward Kinematics Simulation
Figure 3b: Top (left) and side (right) views of the manipulator’s end-pose
Conclusion
Solving for the end-tip position given the joint parameters (or doing forward kinematics) is much easier algebraically when one is using the DH parameters. It is then a lot easier to model a manipulator because four parameters are only needed in order to define a certain joint.
Limitations should also be imposed in the model with respect to the physical environment it belongs to. This is to avoid “impractical” or “illogical” movements being done by the manipulator.
Solving for the inverse kinematics proved to be very difficult algebraically. Multiple solutions often arise and it must always be checked with respect to the physical constraints imposed.