## 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
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
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
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

'''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()
if angular_units=='degrees':
pass
else:
#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.

## Time-lapse Camera with Raspberry Pi

Building a Time-lapse Camera with Raspberry Pi.

Recently I built a time-lapse camera with a Raspberry Pi.  Here’s how:

Bill of Materials

1. Raspberry Pi 3
2. Power Supply
3. Camera Mount
• This ended up having a slightly different mounting hole pattern than the Arducam.
4. Arducam Camera

During initial setup, you’ll also want to have a HDMI Cable, Keyboard, Mouse, and Monitor for your Pi.

Hardware

1. Fasten the Arducam to the camera mount.
2. Connect the Arducam ribbon cable to the Pi’s CSI port.
• Update start time, end time, and sleep interval as desired.
4. (Optional) Update rc.local as mentioned below.

Code

```from time import sleep
from picamera import PiCamera
from datetime import datetime

MORNING_START_HOUR=7
EVENING_END_HOUR=19

def day_or_night(datetime):
hour=datetime.hour
if hour>=MORNING_START_HOUR and hour<EVENING_END_HOUR:
return 'day'
else:
return 'night'

def take_picture():
camera.start_preview()
sleep(2)
now=datetime.now().strftime("%Y-%m-%d-%H-%M")
label='timelapse_' + now + '.jpg'
camera.capture('/home/pi/Pictures/'+label)
camera.stop_preview()
print('Image captured at '+now)

if __name__=='__main__':
camera = PiCamera()

while True:
now=datetime.now()
if day_or_night(now) is 'day':
try:
take_picture()
except:
pass
sleep(900) #15 minutes
```

Also on GitHub.  I was able to get the code to execute upon startup by updating the Pi’s rc.local file.  I followed the rc.local method shown here.

The images are saved in /home/pi/Pictures/ on the Pi.  I used ImageMagick to create the GIF of the plant shown above.

#### Future Improvements

• Saving the files to Google Drive to avoid file storage limitations.  Also, you can view the images without disturbing the camera system.  Looks like this article points us in the right direction.
• Utilizing a portable power supply.

## Hole/Shaft Tolerance Calculator

Calculate Hole/Shaft Tolerances using Excel.

Here’s is an Excel-based calculator for determining the tolerances required to achieve standard metric hole/shaft fits.  Download the calculator here.

As shown in the image above, the user inputs a basic size (Cell B3) and selects the desired fit from the dropdown menu (Cell A3).  The upper and lower limits are instantly calculated for both the shaft and bore.  The results are displayed in the cells highlighted green.

All calculations are based on the methods described in Shigley’s Mechanical Engineering Design (9th Edition).

Posted on Categories Engineering, Excel

## Building a Superflight Controller

Building a controller for Superflight with Arduino, PySerial, and a Wii Nunchuk. A few months ago, I downloaded Superflight on Steam.  It’s an awesome game.

I thought it might be fun to play with a a joystick, but I didn’t have one… so I hacked one together with an Arduino Mega, an old Wii Nunchuk, and PySerial.  The controller works by using the Arduino as an interface between the Nunchuk and computer (via USB) which allows our Python code to read & interpret the Nunchuk data and simulate keystrokes in the game.  The entire hardware configuration and most of the code I needed was already generously available from Gabriel Bianconi’s Makezine article and Chris Kiehl’s Virtual Keystroke project.

The only real hardware change I made was the Arduino pin locations.  For me on an Arduino Mega 2560, this was SDA: Pin 20 and SCL: Pin 21.

In the Arduino code from Bianconi’s article, I modified the Baud rate from 19200 to 9600.  This seemed to be more stable for me, but I’m not sure if it was entirely necessary.  Regardless of what rate you select, make sure the Baud rate matches in the Arduino and Python code.

I pruned a lot of the Nunchuk gyroscopic readings out of Bianconi’s Python code.  Then I added the VK_CODE dictionary (partial) and “press” function from Chris Kiehls’ project which takes advantage of the win32api to simulate keystrokes on a Windows machine.  Finally, I modified some of the existing logic to “press” the arrow keys when the Wii joystick was moved in the corresponding direction.  My python code ended up looking like this:

```"""
Building a Superflight Controller with a Wii Nunchuk

Note: Must run with Python 2.
"""

# Import the required libraries for this script
import string, time, serial, win32api, win32con

#Dictonary to hold key name and VK value
VK_CODE = {'left_arrow':0x25,
'up_arrow':0x26,
'right_arrow':0x27,
'down_arrow':0x28,}

#press keys
def press(*args):
'''
one press, one release.
accepts as many arguments as you want. e.g. press('left_arrow', 'a','b').
'''
for i in args:
win32api.keybd_event(VK_CODE[i], 0,0,0)
time.sleep(.001)
win32api.keybd_event(VK_CODE[i],0 ,win32con.KEYEVENTF_KEYUP ,0)

# The port to which your Arduino board is connected
port = 'COM3'

# Invert y-axis (True/False)
invertY = False

# The cursor speed
cursorSpeed = 20

# The baudrate of the Arduino program
baudrate = 9600

# Variables indicating whether the mouse buttons are pressed or not
leftDown = False
rightDown = False

# Variables indicating the center position (no movement) of the controller
midAnalogY = 130
midAnalogX = 125

if port == 'arduino_port':
print('Please set up the Arduino port.')
while 1:
time.sleep(1)

# Connect to the serial port
ser = serial.Serial(port, baudrate, timeout = 1)

# Wait 5s for things to stabilize
time.sleep(5)

# While the serial port is open
while ser.isOpen():

# Strip the ending (\r\n)
line = string.strip(line, '\r\n')

# Split the string into an array containing the data from the Wii Nunchuk
line = string.split(line, ' ')

print(line)

# Set variables for each of the values
analogX = int(line)
analogY = int(line)
zButton = int(line)

threshold=25

# If the analog stick is not centered
if((analogY-midAnalogY)>threshold):
press('up_arrow')
elif((analogY-midAnalogY)threshold):
press('right_arrow')
elif((analogX-midAnalogX):
press('left_arrow')

# After the program is over, close the serial port connection
ser.close()
```

To summarize, the overall process looks something like this:

1. Connect the Wii Nunchuk to the Arduino as shown in the Makezine article.  Make sure you wire the Nunchuk to your SDA and SCL pins – these might be different that what’s shown in the article depending on what Arduino model you have.
2. Connect the Arduino to your computer through USB and upload Bianconi’s Arduino sketches.  Take note of what baud rate you’re using.
3. Save the python code (shown above) to your local machine.  Update the baud rate as needed – make sure it’s the same as what is listed in your Arduino code.  Make sure you have all python library dependencies installed.
4. Open a terminal.  CD to whatever directory you saved the python code to.  Run the .py file using Python 2.  If you attempt to run it with Python 3, it probably won’t work.
5. Open Superflight and have fun.

#### A few closing thoughts:

• The overall setup is still a little bit unstable.  The python code seems to crash after a few minutes.  A few parameters to troubleshoot with are the threshold variable, the sleep duration, and the baud rate.
• A definite improvement would be to make the Nunchuk trigger buttons work in the menu for a more complete controller.  But the keyboard still works.
• Another big improvement would be to re-write the python code to use a variable rate of virtual button-pressing based on how far from the origin the controller is.
• For a more long-term hardware design, we could design & 3D-print an enclosure that houses an ATtiny which runs the Arduino code.  From the outside, it would just look like a Nunchuk-to-USB cable.
• Maybe it would have been more interesting to use the Wii Gyro data instead of the joystick?

## Find McMaster Details with AutoHotkey

Using AutoHotkey to open a URL.

AutoHotkey script for re-assigning the F6 function key to look up a selected part number on McMaster-Carr.

As you can see below, we’re able to highlight a McMaster part number with and then hit F6 which opens up the appropriate webpage. Prerequisites:

• AutoHotkey installed
• AHK Script is running

Save the follow script as a .ahk file and execute it.  If you want your .ahk scripts to run immediately on system startup, create shortcuts to the .ahk files in your systems startup directory.  For me on Windows 7, this is in C:\Users\Craig\AppData\Roaming\Microsoft\Windows\Start Menu\Programs\Startup.

```
F6:: ; Open McMaster based on selected part number
Clipboard =
Send ^c
ClipWait ;wait for clipboard to have content
Run, chrome.exe https://www.mcmaster.com/#%clipboard%/

```

Thanks to Tom Sherman, McMaster now also supports OpenSearch!

## Web Scraping for Engineers

Scrape 3D models from McMaster-Carr with Python & Selenium.

Here is a script for fetching 3D models from McMaster-Carr using Selenium.

```# -*- coding: utf-8 -*-
"""
Scrape 3D models from McMaster-Carr.

Requirements: Chromedriver.exe is in the same folder as this script.
"""
from selenium import webdriver
import time

test_part_numbers=['98173A200', '7529K105', '93250A440']

def fetch_model(part_numbers, delay=3):
if type(part_numbers) is str:
part_numbers=[part_numbers]

#Start browser
options = webdriver.ChromeOptions()
driver = webdriver.Chrome(chrome_options=options)

#For each part number
for part_number in part_numbers:
driver.get('https://www.mcmaster.com/#' + str(part_number) + '/')
time.sleep(delay)
#Find and Click submit button
try:
try:
submit_button=driver.find_element_by_class_name("button-save--sidebyside")
except:
submit_button=driver.find_element_by_class_name("button-save--stacked")
finally:
submit_button.click()
except:
print('No button found or other error occured')
finally:
time.sleep(delay)

driver.close()
driver.quit

fetch_model(test_part_numbers)
```