## MATLAB Class: Robot Arm Kinematics

MATLAB class of functions for analyzing serial kinematic chain manipulators

Here’s a MATLAB class of functions for analyzing serial kinematic chain manipulators. It includes functions for:

• Forward manipulator kinematics (up to 6 degrees of freedom)
• Denavit-Hartenberg (DH) matrix generation
• Cubic polynomial trajectory generation
• Homogeneous transformation matrix generation
• Planar arm forward & inverse kinematics (from geometry)

To use any of these functions, save the entire class as a .m file in the same directory as your script. Then call RobotKinematics.FunctionName(args).

``````classdef RobotKinematics
% Kinematics and trajectory planning functions for manipulator arms.
%
% Usage: Call RobotKinematics.FunctionName(args)

methods(Static)
%% 6-DOF Manipulator: Forward Kinematics
function [P]=forKin(qdes,joint_num,A)
% Forward Kinematics for Manipulator Arm (up to 6 DOF) in terms
% of CF0.
%
% qdes: Vector of desired joint positions (radians and meters)
% joint_num: Joint of Interest (integer)
% A: 3D array of 2D Frame Transformation A-matrices (DH)

syms theta1 theta2 theta3 theta4 theta5 theta6;
theta=[theta1 theta2 theta3 theta4 theta5 theta6];
theta=theta(1:joint_num);

% Calculate T-matrix (Composite Frame Transformation Matrix
% (DH))
T_local=computeT(A,joint_num);
T_local=subs(T_local,theta(1:joint_num),qdes(1:joint_num));
% Substitute value for pi in case symbol was used
T_local=subs(T_local,pi,3.14159265359);
P=vpa(T_local(1:3,4),3);
end
%% Denavit-Hartenberg (DH) Matrices
function [T] = computeT(A,n)
% Calculate Composite Frame Transformation Matrix (DH)
%
% A: 3D array of 2D Frame Transformation A-matrices (DH)
n=size(A,3);
for ii=1:n
%Calculate Composite Transformation Matrix
if ii==1
T=A(:,:,ii);
else
T=T*A(:,:,ii);
end
end
end

function [A] = computeA(theta,d,a,alpha)
% Calculate Frame Transformation Matrix (DH)
%
% DH Parameters:
% d: Translation about z-axis (meters)
% a: Translation about x-axis (meters)
A=computeRzh(theta)*computeTzh(d)*computeTxh(a)*computeRxh(alpha);
end
%% Trajectory Generation: Cubic Polynomial
function [a]=a_coeffs(t0,tf,q0,qf,dq0,dqf)
% Solve for a-coefficients required for cubic polynomial
% for defining position (p.197, Spong)
%
% t0: Start time (seconds)
% tf: End time(seconds)
% q0: Start joint position (radians)
% qf: End joint position (radians)
% q0: Start joint velocity (radians/second)
% qf: End joint velocity (radians/second)
a=[q0;                                          %a0
dq0;                                         %a1
(3*(qf-q0)-(2*dq0+dqf)*(tf-t0))/((tf-t0)^2); %a2
(2*(q0-qf)+(dq0+dqf)*(tf-t0))/((tf-t0)^3)];  %a3
end

function [q]=joint_pos(a,t)
% Calculate position from cubic polynomial
%
% a: Vector of a-coefficients required for cubic polynomial
% t: time (seconds)
a_flip=flip(a); %Put in descending order (highest order term first)
q=polyval(a_flip,t);
end

function [dq]=joint_vel(a,t)
% Calculate velocity from cubic polynomial
%
% a: Vector of a-coefficients required for cubic polynomial
% t: time (seconds)
a_flip=flip(a); %Put in descending order (highest order term first)
a3=a_flip(1); a2=a_flip(2); a1=a_flip(3);
dq=a1+2*a2*t+3*a3*(t.^2);
end

function [ddq]=joint_accel(a,t)
% Calculate acceleration from cubic polynomial
%
% a: Vector of a-coefficients required for cubic polynomial
% t: time (seconds)
a_flip=flip(a); %Put in descending order (highest order term first)
a3=a_flip(1); a2=a_flip(2);
ddq=2*a2+6*a3*t;
end
%% Rotation and Transformation Matrices
function [Rxh] = computeRxh(theta)
% Compute Basic Homogeneous Transform Matrix for
% rotation of theta (radians) about x-axis
Rxh=[1 0 0 0; ...
0 cos(theta) -sin(theta) 0; ...
0 sin(theta) cos(theta) 0; ...
0 0 0 1;];
end

function [Ryh] = computeRyh(theta)
% Compute Basic Homogeneous Transform Matrix for
% rotation of theta (radians) about y-axis
Ryh=[cos(theta) 0 sin(theta) 0; ...
0 1 0 0; ...
-sin(theta) 0 cos(theta) 0; ...
0 0 0 1];
end

function [Rzh] = computeRzh(theta)
% Compute Basic Homogeneous Transform Matrix for
% rotation of theta (radians) about z-axis
Rzh=[cos(theta) -sin(theta) 0 0; ...
sin(theta) cos(theta) 0 0; ...
0 0 1 0; ...
0 0 0 1];
end

function [Txh] = computeTxh(d)
% Calculate Basic Homogeneous Transform Matrix for
% translation of d (meters) along x-axis
Txh=[1 0 0 d; ...
0 1 0 0; ...
0 0 1 0; ...
0 0 0 1];
end

function [Tyh] = computeTyh(d)
% Calculate Basic Homogeneous Transform Matrix for
% translation of d (meters) along y-axis
Tyh=[1 0 0 0; ...
0 1 0 d; ...
0 0 1 0; ...
0 0 0 1];
end

function [Tzh] = computeTzh(d)
% Calculate Basic Homogeneous Transform Matrix for
% translation of d (meters) along z-axis
Tzh=[1 0 0 0; ...
0 1 0 0; ...
0 0 1 d; ...
0 0 0 1];
end
%% Planar Arm: Forward & Inverse Kinematics
function [x,y]=planar_forKin(q,l)
% Calculate forward kinematics for Planar Arm
% robot geometrically.
%
% q: Vector of Joint Positions (radians)
% l: Vector of Link Lengths (meters)
q1=q(1);q2=q(2);l1=l(1);l2=l(2);
x=l1*cos(q1)+l2*cos(q1+q2);
y=l1*sin(q1)+l2*sin(q1+q2);
end

function [q1,q2]=planar_invKin(x,y,l,elbow_up)
% Calculate inverse kinematics for Planar Arm robot
% geometrically.
%
% x: End Effector x-Position (meters)
% y: End Effector y-Position (meters)
% l: Vector of Link Lengths (meters)
% elbow_up: 0 or 1
l1=l(1);l2=l(2);
D=(x^2+y^2-l1^2-l2^2)/(2*l1*l2);
if elbow_up==1
q2=atan2(-sqrt(1-D^2),D);
else %elbow is down
q2=atan2(sqrt(1-D^2),D);
end
q1=atan2(y,x)-atan2((l2*sin(q2)),(l1+l2*cos(q2)));
end
end
end
``````

Also available on GitHub.

## Transformation Matrices for Robotic Arms

Python functions for serial manipulators.

``````# -*- coding: utf-8 -*-
"""
Functions for calculating Basic Transformation Matrices in 3D space.
"""
from math import cos, radians, sin
from numpy import matrix

def rotate(axis, theta, angular_units='radians'):
'''Compute Basic Homogeneous Transform Matrix for
rotation of "theta" about specified axis.'''
#Verify string arguments are lowercase
axis=axis.lower()
angular_units=angular_units.lower()
#Convert to radians if necessary
if angular_units=='degrees':
pass
else:
raise Exception('Unknown angular units.  Please use radians or degrees.')
#Select appropriate basic homogenous matrix
if axis=='x':
rotation_matrix=matrix([[1, 0, 0, 0],
[0, cos(theta), -sin(theta), 0],
[0, sin(theta), cos(theta), 0],
[0, 0, 0, 1]])
elif axis=='y':
rotation_matrix=matrix([[cos(theta), 0, sin(theta), 0],
[0, 1, 0, 0],
[-sin(theta), 0, cos(theta), 0],
[0, 0, 0, 1]])
elif axis=='z':
rotation_matrix=matrix([[cos(theta), -sin(theta), 0, 0],
[sin(theta), cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
else:
raise Exception('Unknown axis of rotation.  Please use x, y, or z.')
return rotation_matrix

def translate(axis, d):
'''Calculate Basic Homogeneous Transform Matrix for
translation of "d" along specified axis.'''
#Verify axis is lowercase
axis=axis.lower()
#Select appropriate basic homogenous matrix
if axis=='x':
translation_matrix=matrix([[1, 0, 0, d],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
elif axis=='y':
translation_matrix=matrix([[1, 0, 0, 0],
[0, 1, 0, d],
[0, 0, 1, 0],
[0, 0, 0, 1]])
elif axis=='z':
translation_matrix=matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, d],
[0, 0, 0, 1]])
else:
raise Exception('Unknown axis of translation.  Please use x, y, or z.')
return translation_matrix

if __name__=='__main__':
#Calculate arbitrary homogeneous transformation matrix for CF0 to CF3
H0_1=rotate('x', 10, 'degrees')*translate('y', 50)
H1_2=rotate('y', 30, 'degrees')*translate('z', 10)
H2_3=rotate('z', -20, 'degrees')*translate('z', 10)
H0_3=H0_1*H1_2*H2_3
print(H0_3)
``````

Also available on GitHub.