Derive and Apply Inverse Kinematics to Two-Link Robot Arm

Open Live Script

This example derives and applies inverse kinematics to a two-link robot arm by using MATLAB® and Symbolic Math Toolbox™.

The example defines the joint parameters and end-effector locations symbolically, calculates and visualizes the forward and inverse kinematics solutions, and finds the system Jacobian, which is useful for simulating the motion of the robot arm.

Step 1: Define Geometric Parameters

Define the link lengths, joint angles and end-effector locations of the robots as symbolic variables.

symsL_1L_2theta_1theta_2XEYE

Specify values for the link lengths of the robot.

L1 = 1; L2 = 0.5;

Step 2: Define X and Y Coordinates of End Effector

Define the X and Y coordinates of the end-effector as a function of the joint angles $(θ_{1}, θ_{2})$ .

XE_RHS = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2)

XE_RHS =
                   $L_{2} \cos (θ_{1} + θ_{2}) + L_{1} \cos (θ_{1})$

YE_RHS = L_1*sin(theta_1) + L_2*sin(theta_1+theta_2)

YE_RHS =
                   $L_{2} \sin (θ_{1} + θ_{2}) + L_{1} \sin (θ_{1})$

Convert the symbolic expressions into MATLAB functions.

XE_MLF = matlabFunction(XE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]); YE_MLF = matlabFunction(YE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]);

Step 3: Calculate and Visualize Forward Kinematics

Forward kinematics transforms the joint angles into end-effector locations: $(θ_{1}, θ_{2}) ⟶ f (θ_{1}, θ_{2}) ⟶ (X_{E}, Y_{E})$ . Given specific joint-angle values, use forward kinematics to calculate the end-effector locations.

Specify the input values of the joint angles as $0^{\circ} < θ_{1} < 90^{\circ}$ and ${- 180}^{\circ} < θ_{2} < 180^{\circ}$ .

t1_degs_row = linspace(0,90,100); t2_degs_row = linspace(-180,180,100); [tt1_degs,tt2_degs] = meshgrid(t1_degs_row,t2_degs_row);

Convert the angle units from degrees to radians.

tt1_rads = deg2rad(tt1_degs); tt2_rads = deg2rad(tt2_degs);

Calculate the X and Y coordinates using the MATLAB functionsXE_MLFandYE_MLF, respectively.

X_mat = XE_MLF(L1,L2,tt1_rads,tt2_rads); Y_mat = YE_MLF(L1,L2,tt1_rads,tt2_rads);

Visualize the X and Y coordinates using the helper functionplot_XY_given_theta_2dof.

plot_XY_given_theta_2dof(tt1_degs,tt2_degs,X_mat,Y_mat,(L1+L2))

Figure contains 2 axes objects. Axes object 1 with title X indexOf E baseline contains an object of type contour. Axes object 2 with title Y indexOf E baseline contains an object of type contour.

Step 4:Find Inverse Kinematics

Inverse kinematics transforms the end-effector locations into joint angles: $(X_{E}, Y_{E}) ⟶ g (X_{E}, Y_{E}) ⟶ (θ_{1}, θ_{2})$ . Find the inverse kinematics from the forward kinematics equations.

Define the forward kinematics equations.

XE_EQ = XE == XE_RHS; YE_EQ = YE == YE_RHS;

Solve for $θ_{1}$ and $θ_{2}$ .

S = solve([XE_EQ YE_EQ], [theta_1 theta_2])

S =struct with fields:theta_1: [2x1 sym] theta_2: [2x1 sym]

The structureSrepresents the multiple solutions for $θ_{1}$ and $θ_{2}$ . Show the pair of solutions for $θ_{1}$ .

simplify(S.theta_1)

ans =
                  
                    $\begin{array}{l} (\begin{array}{c} 2 atan (\frac{2 L_{1} YE + σ_{1}}{{L_{1}}^{2} + 2 L_{1} XE - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}}) \\ 2 atan (\frac{2 L_{1} YE - σ_{1}}{{L_{1}}^{2} + 2 L_{1} XE - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}}) \end{array}) \\ where \\ σ_{1} = \sqrt{- {L_{1}}^{4} + 2 {L_{1}}^{2} {L_{2}}^{2} + 2 {L_{1}}^{2} {XE}^{2} + 2 {L_{1}}^{2} {YE}^{2} - {L_{2}}^{4} + 2 {L_{2}}^{2} {XE}^{2} + 2 {L_{2}}^{2} {YE}^{2} - {XE}^{4} - 2 {XE}^{2} {YE}^{2} - {YE}^{4}} \end{array}$

Show the pair of solutions for $θ_{2}$ .

simplify(S.theta_2)

ans =
                  
                    $\begin{array}{l} (\begin{array}{c} - σ_{1} \\ σ_{1} \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\sqrt{(- {L_{1}}^{2} + 2 L_{1} L_{2} - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}) ({L_{1}}^{2} + 2 L_{1} L_{2} + {L_{2}}^{2} - {XE}^{2} - {YE}^{2})}}{- {L_{1}}^{2} + 2 L_{1} L_{2} - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}}) \end{array}$

Convert the solutions into MATLAB functions that you can use later. The functionsTH1_MLFandTH2_MLFrepresent the inverse kinematics.

TH1_MLF{1} = matlabFunction(S.theta_1(1),'Vars',[L_1 L_2 XE YE]); TH1_MLF{2} = matlabFunction(S.theta_1(2),'Vars',[L_1 L_2 XE YE]); TH2_MLF{1} = matlabFunction(S.theta_2(1),'Vars',[L_1 L_2 XE YE]); TH2_MLF{2} = matlabFunction(S.theta_2(2),'Vars',[L_1 L_2 XE YE]);

Step 5: Calculate and Visualize Inverse Kinematics

Use the inverse kinematics to compute $θ_{1}$ and $θ_{2}$ from the X and Y coordinates.

Define the grid points of the X and Y coordinates.

[xmat,ymat] = meshgrid(0:0.01:1.5,0:0.01:1.5);

Calculate the angles $θ_{1}$ and $θ_{2}$ using the MATLAB functionsTH1_MLF{1}andTH2_MLF{1}, respectively.

tmp_th1_mat = TH1_MLF{1}(L1,L2,xmat,ymat); tmp_th2_mat = TH2_MLF{1}(L1,L2,xmat,ymat);

Convert the angle units from radians to degrees.

tmp_th1_mat = rad2deg(tmp_th1_mat); tmp_th2_mat = rad2deg(tmp_th2_mat);

Some of the input coordinates, such as (X,Y) = (1.5,1.5), are beyond the reachable workspace of the end effector. The inverse kinematics solutions can generate some imaginary theta values that require correction. Correct the imaginary theta values.

th1_mat = NaN(size(tmp_th1_mat)); th2_mat = NaN(size(tmp_th2_mat)); tf_mat = imag(tmp_th1_mat) == 0; th1_mat(tf_mat) = real(tmp_th1_mat(tf_mat)); tf_mat = imag(tmp_th2_mat) == 0; th2_mat(tf_mat) = real(tmp_th2_mat(tf_mat));

Visualize the angles $θ_{1}$ and $θ_{2}$ using the helper functionplot_theta_given_XY_2dof.

plot_theta_given_XY_2dof(xmat,ymat,th1_mat,th2_mat)

Figure contains 2 axes objects. Axes object 1 with title theta indexOf 1 baseline contains an object of type contour. Axes object 2 with title theta indexOf 2 baseline contains an object of type contour.

Step 6: Compute System Jacobian

The definition of the system Jacobian is:

$J = \frac{d (X, Y)}{d (θ_{1}, θ_{2})} = (\begin{array}{c} \frac{d X}{d θ_{1}} & \frac{d X}{d θ_{2}} \\ \frac{d Y}{d θ_{1}} & \frac{d Y}{d θ_{2}} \end{array})$

the_J = jacobian([XE_RHS YE_RHS],[theta_1 theta_2])

the_J =
                  
                    $(\begin{array}{c} - L_{2} \sin (θ_{1} + θ_{2}) - L_{1} \sin (θ_{1}) & - L_{2} \sin (θ_{1} + θ_{2}) \\ L_{2} \cos (θ_{1} + θ_{2}) + L_{1} \cos (θ_{1}) & L_{2} \cos (θ_{1} + θ_{2}) \end{array})$

You can relate the joint velocity to the end-effector velocity, and the other way around, by using the system Jacobian:

$(\begin{array}{c} \frac{d X}{d t} \\ \frac{d Y}{d t} \end{array}) = (\begin{array}{c} \frac{d X}{d θ_{1}} & \frac{d X}{d θ_{2}} \\ \frac{d Y}{d θ_{1}} & \frac{d Y}{d θ_{2}} \end{array}) . (\begin{array}{c} \frac{d θ_{1}}{d t} \\ \frac{d θ_{2}}{d t} \end{array})$

$(\begin{array}{c} \frac{d X}{d t} \\ \frac{d Y}{d t} \end{array}) = J . (\begin{array}{c} \frac{d θ_{1}}{d t} \\ \frac{d θ_{2}}{d t} \end{array})$

$(\begin{array}{c} \frac{d θ_{1}}{d t} \\ \frac{d θ_{2}}{d t} \end{array}) = J^{+} . (\begin{array}{c} \frac{d X}{d t} \\ \frac{d Y}{d t} \end{array}) w h e r e J^{+} is the Moore - Penrose pseudoinverse of J$

You can also convert the symbolic expression of the Jacobian to a MATLAB function block. Simulate the end-effector locations of the robot on a trajectory by defining the multiple waypoints as inputs to a Simulink model. The Simulink model can calculate a motion-profile based on the joint angle values to reach each waypoint in the trajectory. For more details, seeInverse Kinematics of a 2-link Robot ArmandTeaching Rigid Body Dynamics.

Helper Functions

functionplot_theta_given_XY_2dof(X_mat,Y_mat,theta_1_mat_degs,...theta_2_mat_degs) xlab_str ='X (m)'; ylab_str ='Y (m)'; figure; hax(1) = subplot(1,2,1); contourf(X_mat, Y_mat, theta_1_mat_degs); caxis(hax(1), [-180 180]); colormap(gca,'jet'); colorbar xlabel(xlab_str,'Interpreter','tex'); ylabel(ylab_str,'Interpreter','tex'); title(hax(1),'\theta_1','Interpreter','tex') axis('equal') hax(2) = subplot(1,2,2); contourf(X_mat, Y_mat, theta_2_mat_degs); caxis(hax(2), [-180 180]); colormap(gca,'jet'); colorbar xlabel(xlab_str,'Interpreter','tex'); ylabel(ylab_str,'Interpreter','tex'); title(hax(2),'\theta_2','Interpreter','tex') axis('equal')endfunctionplot_XY_given_theta_2dof(theta_1_mat_degs,theta_2_mat_degs,...X_mat,Y_mat,a_cmax) xlab_str ='\theta_1 (degs)'; ylab_str ='\theta_2 (degs)'; figure; hax(1) = subplot(1,2,1); contourf(theta_1_mat_degs, theta_2_mat_degs, X_mat); caxis(hax(1), [0 a_cmax]); colormap(gca,'jet'); colorbar xlabel(xlab_str,'Interpreter','tex'); ylabel(ylab_str,'Interpreter','tex'); title(hax(1),'X_E','Interpreter','tex') hax(2) = subplot(1,2,2); contourf(theta_1_mat_degs, theta_2_mat_degs, Y_mat); caxis(hax(1), [0 a_cmax]); colormap(gca,'jet'); colorbar xlabel(xlab_str,'Interpreter','tex'); ylabel(ylab_str,'Interpreter','tex'); title(hax(2),'Y_E','Interpreter','tex')end