1

Model of Robotic Manipulator with Revolute and

Prismatic Joints

03/25/2016

Travis Heidrich

Report Contents:

Dimensions of the Robot 2

Denavit-Hartenberg Parameters 2-3

MATLAB Script 3-5

Frame Transformation Matrices 6

Reachable Workspace for Manipulator 7-8

Inverse Kinematics for Manipulator 8-9

Jacobian and Singularity 10

Angular and Linear Velocities, Forces, and Torques of Robot 11

2

Dimensions of Robot

L1 = 1 m; L2 = 0.5 sin(45°) = 0.3536 m; LE = 0.5 m

D-H Parameters

Figure 1: Reference Frames and Dimensions of the three link robot

3

Table 1: D-H Parameter Table for all frames of the robot

i αi - 1 ai - 1 di - 1 θi

1 0 0 L1 θ1 + 135°

2 L2 90° d2 0

3 0 0 0 θ3

E 0 -90° LE 0

MatLab Script

%Solutions clear all close all

%Robot Dimensions

L_1 = 1; %meters

L_2 = 0.5*sind(45); %meters L_E = 0.5; %meters

syms ai alphai di thetai theta1 d_2 theta3 T_x=[1 0 0 0;0 cos(alphai) -sin(alphai) 0;0 sin(alphai) cos(alphai) 0;0 0 0

1]; D_x=[1 0 0 ai;0 1 0 0;0 0 1 0;0 0 0 1]; T_z=[cos(thetai) -sin(thetai) 0 0;sin(thetai) cos(thetai) 0 0;0 0 1 0 ;0 0 0

1]; D_z=[1 0 0 0;0 1 0 0;0 0 1 di;0 0 0 1];

%Link the reference frames into one homogeneous transform matrix AtB=T_x*D_x*T_z*D_z;

%Transformation from frame 0 to frame 1 ai = 0; alphai = 0; di = L1; thetai = theta1 + 3*pi/4; T_01 = subs(AtB); iT_01 = inv(T_01);

%Transformation from frame 1 to frame 2 ai = L_2; alphai = pi/2; di = d_2; thetai = 0; T_12 = subs(AtB); %iT_12 = inv(T_12);

%Transformation from frame 2 to frame 3 ai = 0;

4

alphai = 0; di = 0; thetai = theta3; T_23 = subs(AtB);

%Transformation from frame 3 to frame E ai = 0; alphai = -pi/2; di = LE; thetai = 0;

T3E = subs(AtB); %iT3E = inv(T3E);

% Linking the Transformations T_02 = T_01*T_12; T_03 = T_01*T_12*T_23; T_0E = T_01*T_12*T_23*T_3E; T_1E = T_12*T_23*T_3E; T_2E = T_23*T_3E;

x = T_0E(1,4); y = T_0E(2,4); z = T_0E(3,4);

%Inverse Kinematics for robot syms px py pz theta3 = solve( pz == z, theta3); [d_2, theta1] = solve(px == x, py == y, d_2, theta1);

%Rotation Matrices frame to frame R_01 = T_01(1:3,1:3); R_12 = T_12(1:3,1:3); R_23 = T_23(1:3,1:3); R_3E = T_3E(1:3,1:3); R_02 = R_01*R_12; R_03 = R_01*R_12*R_23; R_0E = R_01*R_12*R_23*R_3E;

%Forces 10,5,and 1 f_EE = [10;5;1]; f_33 = R_3E*f_EE; f_22 = R_23*f_33; f_11 = R_12*f_22; f_00 = R_01*f_11;

%Vectors between D-H Origins P_01 = T_01(1:3,4); P_12 = T_12(1:3,4); P_23 = T_23(1:3,4); P_3E = T_3E(1:3,4); P_00 = [0;0;0]; P_02 = T_02(1:3,4); P_03 = T_03(1:3,4); P_0E = T_0E(1:3,4);

5

%Robot Torques n_EE = zeros(3,1); n_33 = R_01*n_EE + cross(P_3E,f_33); n_22 = R_01*n_33 + cross(P_23,f_22); n_11 = R_01*n_22 + cross(P_12,f_11); n_00 = R_01*n_11 + cross(P_01,f_00);

t_0 = transpose(n_00)*[0;0;1]; t_1 = transpose(n_11)*[0;0;1]; t_2 = transpose(f_22)*[0;0;1]; t_3 = transpose(n_33)*[0;0;1]; t_E = zeros(3,1);

%Velocities syms theta1Dot theta2Dot theta3Dot d_2Dot

%Unit Vectors u_i = [1;0;0]; u_j = [0;1;0]; u_k = [0;0;1];

%Angular rotations z_00 = u_k; z_01 = R_01*u_k; z_02 = R_02*u_k; %Prismatic z_03 = R_03*u_k;

J_angular = [z00, 0*z01, z02]; Omega_0E = theta1Dot*u_k + 0 + theta3Dot*R_02*u_k + 0;

%Linear J_linear = [cross(z_00,P_0E – P_00), z_02, cross(z_02,P_0E – P_02)];

%linear velocity equations W_00 = zeros(3,1); w_11 = inv(R_01)*w_00+[0;0;1]*theta1Dot; w_22 = inv(R_12)*w_11+[0;0;0]*theta2Dot; w_33 = inv(R_23)*w_22+[0;0;1]*theta3Dot; w_EE = zeros(3,1);

v_00 = zeros(3,1); v_11 = inv(R_01)*(v_00+cross(w_00,P_01)); v_22 = inv(R_12)*(v_11+cross(w_11,P_12))+d_2Dot*[0;0;1]; %Prismatic v_33 = inv(R_23)*(v_22+cross(w_22,P_23)); v_EE = inv(R_3E)*(v_33+cross(w_33,P_3E)); v_0E = R_0E*v_EE;

6

Frame Transformation Matrices

𝑇01 =

[ −sin (

𝜋

4+ 𝜃1) −cos (

𝜋

4+ 𝜃1) 0 0

cos (𝜋

4+ 𝜃1) − sin (

𝜋

4+ 𝜃1) 0 0

0 0 1 10 0 0 1]

𝑇12 =

[ 1 0 0

√2

40 0 −1 −𝑑2

0 1 0 00 0 0 1 ]

𝑇23 = [

cos(𝜃3) −sin(𝜃3) 0 0

sin(𝜃3) cos(𝜃3) 0 00 0 1 00 0 0 1

]

𝑇3𝐸 =

[ 1 0 0 0

0 0 11

20 0 1 00 0 0 1]

𝑇0𝐸 =

[ − cos(𝜃3) ∗ sin (

𝜋

4+ 𝜃1) −cos (

𝜋

4+ 𝜃1) sin(𝜃3) ∗ sin (

𝜋

4+ 𝜃1)

(sin(𝜃3) ∗ sin (𝜋4

+ 𝜃1))

2+ 𝑑2 ∗ cos (

𝜋

4+ 𝜃1) −

(√2 ∗ sin (𝜋4

+ 𝜃1))

4

cos(𝜃3) ∗ sin (𝜋

4+ 𝜃1) −𝑠𝑖𝑛 (

𝜋

4+ 𝜃1) − cos (

𝜋

4+ 𝜃1) ∗ sin(𝜃3)

(√2 ∗ cos (𝜋4

+ 𝜃1))

4−

(cos (𝜋4

+ 𝜃1) ∗ sin(𝜃3))

2+ 𝑑2 ∗ sin (

𝜋

4+ 𝜃1)

sin(𝜃3) 0 cos (𝜃3)cos (𝜃3)

2+ 1

0 0 0 1 ]

7

Reachable Workspace for Manipulator

Figure 2: X-Z Plane View of reachable workspace

Figure 3: X-Y Plane View of reachable workspace

8

Figure 4: Trimetric view of reachable workspace

Inverse Kinematics for Manipulator

𝑇0𝐸 = [

𝑟11 𝑟12 𝑟13 𝑝𝑥

𝑟21 𝑟22 𝑟23 𝑝𝑦

𝑟31 𝑟32 𝑟33 𝑝𝑧

0 0 0 1

]

𝑝𝑧 =cos (𝜃3)

2+1

𝑝𝑦 = (√2 𝑐𝑜𝑠 (

𝜋4 + 𝜃1))

4−

( 𝑐𝑜𝑠 (𝜋4 + 𝜃1) 𝑠𝑖𝑛(𝜃3))

2+ 𝑑2 𝑠𝑖𝑛 (

𝜋

4+ 𝜃1)

𝑝𝑥 =( 𝑠𝑖𝑛(𝜃3) 𝑠𝑖𝑛 (

𝜋4 + 𝜃1))

2−

(√2 𝑠𝑖𝑛 (𝜋4 + 𝜃1))

4+ 𝑑2 𝑐𝑜𝑠 (

𝜋

4+ 𝜃1)

Solving for θ3

𝜃3 = ±cos−1(2𝑝𝑧 − 2)

Substituting θ3 and solving for θ1 and d2

𝜃1 = 2 tan−1 (4𝑝𝑦 ± √2√(8𝑝𝑥2 − 2√2 sin(𝜃3) + cos(2𝜃3) + 8𝑝𝑦

2 − 2)) −3𝜋

4

9

𝑑2 = ±

(√2√(8𝑝𝑥2 + 8𝑝𝑦

2 + cos(2𝜃3) + 2√2 sin(𝜃3) − 2))

4

Solutions for the point: (-0.275, 0.6125, 1.4375)

𝜃3 = ± 0.5054 𝑟𝑎𝑑

𝜃1

𝑑2= {

−5.2426 0.1163 −1.7674 −0.8431 𝑟𝑎𝑑0.6621 0.3099 −0.6621 −0.3099 𝑚

Solution set to satisfy the limits is

𝜃3 = 0.5054 𝑟𝑎𝑑

𝜃1 = −5.2426 𝑟𝑎𝑑 = 1.0406 𝑟𝑎𝑑

𝑑2 = 0.6621 𝑚

Where d2 for both the offset L2 and the 0.1 m limit.

𝑑2 − 𝐿2 − 0.1 = 0.2085 𝑚

Figure 5: MatLab output of robot configured with the inverse kinematic solution

10

Jacobian

[ cos (

𝜋4

+ 𝜃1) ∗ sin(𝜃3)

2−

√2 ∗ cos (𝜋4

+ 𝜃1)

4− 𝑑2 ∗ sin (

𝜋

4+ 𝜃1) cos (

𝜋

4+ 𝜃1)

cos(𝜃3) ∗ sin (𝜋4

+ 𝜃1)

2

sin(𝜃3) ∗ sin (𝜋4

+ 𝜃1)

2−

√2 ∗ sin (𝜋4

+ 𝜃1)

4+ 𝑑2 ∗ cos (

𝜋

4+ 𝜃1) sin(

𝜋

4+ 𝜃1) −

cos(𝜃3) ∗ cos (𝜋4

+ 𝜃1)

2

0 0− sin(𝜃3)

2

0 0 −cos (𝜃

4+ 𝜃1)

0 0 sin (𝜃

4+ 𝜃1)

1 0 0 ]

𝐷𝑒𝑡(𝐽𝑙𝑖𝑛𝑒𝑎𝑟) = 𝑑2 ∗ sin(𝜃3)

2

𝐷𝑒𝑡(𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟) = 0

Singularities

When determinate of Jacobian = 0

The was no determinate for the angular Jacobian

There was no theta 1 that resulted in singularities

Given theta3 = 0 and pi

For d_2 < = 0.5(sin(45)) + 0.1

OR

d_2 > 0.5(sin(45)) + 0.5

11

Angular and Linear Velocities, Forces, and

Torques of Robot

Table 2: The angular velocity of the end effector

𝜔0𝐸 �̇�3 ∗ sin (

3𝜋

4+ 𝜃1) −�̇�3 ∗ cos (

3𝜋

4+ 𝜃1)

�̇�1

Table 3: The linear velocity of the end effector

𝑣0𝐸 𝑣𝑥 =�̇�1∗sin (𝜃1)

4−

�̇�1∗cos(𝜃1)

4−

√2∗�̇�2∗sin(𝜃1)

2+

√2∗�̇�2∗cos(𝜃1)

2−

√2∗𝜃1̇∗sin(𝜃1)∗sin(𝜃3)

4−

(√2∗𝑑2∗𝜃1̇∗cos(𝜃1))

2 −

(√2∗𝑑2∗𝜃1̇∗sin(𝜃1))

2+

√2∗�̇�3∗cos(𝜃1)∗cos(𝜃3)

4+

√2∗�̇�1∗cos(𝜃1)∗sin(𝜃3)

4

+√2∗�̇�3∗cos(𝜃3)∗sin(𝜃1)

4

𝑣𝑦 =√2∗�̇�2∗sin(𝜃1)

2−

�̇�1∗sin(𝜃1)

4−

�̇�1∗cos(𝜃1)

4+

√2∗�̇�2∗cos(𝜃1)

2+

√2∗�̇�1∗sin(𝜃1)∗sin(𝜃3)

4+

√2∗𝑑2∗�̇�1∗cos(𝜃1)

2−

√2∗𝑑2∗�̇�1∗sin(𝜃1)

2−

√2∗�̇�3∗cos(𝜃1)∗cos(𝜃3)

4+

√2∗�̇�1∗cos(𝜃1)∗sin(𝜃3)

4+

√2∗𝑑2∗�̇�3∗cos(𝜃3)∗sin (𝜃1)

4

𝑣𝑧 = −𝜃3̇sin (𝜃3)

2

Table 4: The forces for each link with respect to each link’s frame in N

End Effector 𝐹𝑥 = 10 𝐹𝑦 = 5 𝐹𝑧 = 1

Third Link 𝐹𝑥 = 10 𝐹𝑦 = 1 𝐹𝑧 = −5

Second Link 𝐹𝑥 = 8.266 𝐹𝑦 = 5.716 𝐹𝑧 = −5

First Link 𝐹𝑥 = 8.266 𝐹𝑦 = 5 𝐹𝑧 = 5.716

Base 𝐹𝑥 = -6.736 𝐹𝑦 =-6.925 𝐹𝑧 = 5.716

Table 5: Each joint torque in Nm:

End Effector 𝜏𝐸 = 0

Third Link 𝜏3 = −5

Second Link 𝜏2 = −5

First Link 𝜏1 =2.240

Base 𝜏𝑩 = 0