Robotics 2

1. Spherical Wrists

SCARA PRR Variant 3

3-DOF

6-DOF

2. D-H Notation

Example:

Step 3: Plug the table into the Homogeneous Transformation Matrix formula.

Step 4: Multiply the matrices together

3. Forward Kinemtics

Example:

Any of the 2 processes, it will still arrive on the same answer.

Determining the position and direction using Forward Kinematics.

Simulation and visualization of Forward Kinematics using MATLAB:

If θ1=0֯ and θ2=0֯:

If θ1=90֯ and θ2=-90֯:

If θ1=45֯ and θ2=30֯:

3.1 Forward Kinematics in Python and MATLAB

Codes can also be found from: https://github.com/MikkoDT/Robotics-2-2022-2023 And the tutorial for installation of Robotics Toolbox by Peter Corke in Python can be found here: https://youtube.com/playlist?list=PLUgsbeZHs9qMFXTIQPW0clLoRkf_oiBoX&si=0sYu2QIJ4NWilTq7

FKin.py of SCARA-V3

Import librariies first for scientific and matrix computations

import numpy as np

link lengths in mm

a1 = float(input("a1 = "))

a2 = float(input("a2 = "))

a3 = float(input("a3 = "))

a4 = float(input("a4 = "))

joint variables: is mm if f, is degrees if theta

d1 = float(input("d1 = ")) #20 mm

T2 = float(input("T2 = ")) #30 deg

T3 = float(input("T3 = ")) #-90 deg

degree to radian

T2 = (T2/180.0)*np.pi

T3 = (T3/180.0)*np.pi

Parametric Table (theta, alpha, r, d)

PT = [[(0.0/180.0)*np.pi,(0.0/180.0)*np.pi,0,a1+d1], [T2,(0.0/180.0)*np.pi,a2,0],
[T3,(0.0/180.0)*np.pi,a4,a3]]

HTM formula and multiplication

i = 0

H0_1 = [[np.cos(PT[i][0]),-np.sin(PT[i][0])*np.cos(PT[i][1]),np.sin(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.cos(PT[i][0])], [np.sin(PT[i][0]),np.cos(PT[i][0])*np.cos(PT[i][1]),-np.cos(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.sin(PT[i][0])],
[0,np.sin(PT[i][1]),np.cos(PT[i][1]),PT[i][3]],
[0,0,0,1]]

i = 1

H1_2 = [[np.cos(PT[i][0]),-np.sin(PT[i][0])*np.cos(PT[i][1]),np.sin(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.cos(PT[i][0])], [np.sin(PT[i][0]),np.cos(PT[i][0])*np.cos(PT[i][1]),-np.cos(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.sin(PT[i][0])],
[0,np.sin(PT[i][1]),np.cos(PT[i][1]),PT[i][3]],
[0,0,0,1]]

i = 2

H2_3 = [[np.cos(PT[i][0]),-np.sin(PT[i][0])*np.cos(PT[i][1]),np.sin(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.cos(PT[i][0])], [np.sin(PT[i][0]),np.cos(PT[i][0])*np.cos(PT[i][1]),-np.cos(PT[i][0])*np.sin(PT[i][1]),PT[i][2]*np.sin(PT[i][0])],
[0,np.sin(PT[i][1]),np.cos(PT[i][1]),PT[i][3]],
[0,0,0,1]]

Multiply the matrices

H0_1 = np.matrix(H0_1)

print("H0_1= ")

print(H0_1)

H1_2 = np.matrix(H1_2)

print("H1_2= ")

print(H1_2)

H2_3 = np.matrix(H2_3)

print("H2_3= ")

print(H2_3)

H0_2 = np.dot(H0_1,H1_2)

H0_3 = np.dot(H0_2,H2_3)

print("H0_3= ")

print(np.matrix(np.around(H0_3,3)))

MATLAB SCARAV3_Sim.m

disp('SCARA_V3')

syms a1 a2 a3 a4

%% Link lengths

a1 = 5;

a2 = 10;

a3 = 5;

a4 = 10;

%% D-H Parameters [theta, d, r, alpha, offset]

% if prismatic joint: theta = theta, d = 0, offset = 1, after offset put the value of d

% if revolute joint: theta = 0,offset = 0, after offset put the value of theta

H0_1 = Link([0,0,0,0,1,a1]);

H0_1.qlim = [0 30];

H1_2 = Link([0,0,a2,0,0]);

H1_2.qlim = pi/180*[-90 90];

H2_3 = Link([0,a3,a4,0,0]);

H2_3.qlim = pi/180*[-90 90];

Scara_V3 = SerialLink([H0_1 H1_2 H2_3], 'name', 'SCARA_V3')

Scara_V3.plot([0 0 0], 'workspace', [-5 30 -30 30 0 30])

Scara_V3.teach

%% Forward Kinemtics

%syntax: FK = robot_variable.fkine(joint_variables)

Af = ([5,pi/2,pi/2]); %joint_variables FK = Scara_V3.fkine(Af)

5. Inverse Kinematics

Graphical Method for Solving Inverse Kinematics

• Example:

5.1 Inverse Kinematics of Cartesian Manipulator

5.2 Inverse Kinematics of Articulated Manipulator

5.3 Inverse Kinematics in Python

Codes can also be found from: https://github.com/MikkoDT/Robotics-2-2022-2023

IKin.py of SCARA-V3

Inverse Kinematics of SCARA-V3

import numpy as np

link lengths in mm

a1 = float(input("a1 = "))

a2 = float(input("a2 = "))

a3 = float(input("a3 = "))

a4 = float(input("a4 = "))

Position Vector in mm

x0_3 = float(input("x0_3 = "))

y0_3 = float(input("y0_3 = "))

z0_3 = float(input("z0_3 = "))

Inverse Kinematic Solutions using Graphical Method

Solution 1

phi2 = np.arctan(y0_3 / x0_3)

phi2 = phi2 * 180 / np.pi

Solution 2

r1 = np.sqrt((y0_32) + (x0_32))

Solution 3

phi1 = np.arccos((a42 - r12 - a2**2)/(-2 * r1 * a2))

phi1 = phi1*180 / np.pi

Solution 4

theta2 = phi2 - phi1

solution 5

phi3 = np.arccos((r12 - a22 - a4**2)/(-2 * a2 * a4))

phi3 = phi3 * 180 / np.pi

Solution 6

theta3 = 180 - phi3

Solution 7

d1 = z0_3 - a1 - a3

Displaying the Joint Variables

print("d1 = ", np.around(d1,3))

print("theta2 = ", np.around(theta2,3))

print("theta3 = ", np.around(theta3,3))