October 31, 2013

Robot Arm: Testing Construction Methods and More Design

I CADed up a 3D-printable test arm, to see how well adding carbon fiber over ABS plastic would work:


And then printed it in two parts on a Stratasys uPrint.  The two halves were superglued together.  The full part would have just barely fit diagonally on the uPrint's print bed, but I split it into two parts so that I could also print test pieces on my own UP! if needed.


I machined an aluminum end for it:



Next step was adding carbon fiber to the arm.  To compress the carbon and epoxy, I heat-shrinked the wet assembly:


Here it is after a bit of cleanup.  The heatshrink leaves a nice satin surface finish.




The test piece turned out to be pretty much a complete failure.  First, the epoxy I used did not fully cure.  It's the same batch of epoxy I used on my carbon fiber bike, so I'm not sure what the problem was.  Nick has had lots of problems with this epoxy recently as well.  Also, the epoxy did not bond whatsoever to the ABS plastic.  I was able to cut the carbon fiber and peel it off by hand after letting it sit for 4 days.  I think if I redesign the arm such that pretty much the entire surface can be covered in carbon, the carbon delaminating won't be an issue.

Other thoughts:  The whole assembly is much heavier than I would like.  The aluminum attachment point at the end should be smaller, and the printed walls could be made thinner.  This version was printed with 2 mm thick walls, but thinner would be fine with carbon fiber added.  The plastic serves two purposes, which are to provide a precisely sized mold for the carbon fiber, and interfaces for bearings.  It should not be taking much of the bending load, so the walls can be made as thin as possible.

Also, with newly acquired TechX funding, I've started ordering things like bearings, belts pulleys.  Most of the parts were found on SDP-SI, and then ordered on Ebay for significantly less cost.  To figure out the pulley sizes, I plugged in the moment of inertia figures for my arm CAD model in Solidworks to my gear ratio-optimizing script, and then found the closest sized pulleys I could actually buy.  Rather than using the single extremely large tooth-count pulley recommended by the script, which I would have to machine myself and would be awkwardly large in diameter, I have two-stage reductions with smaller pulleys.

The relevant moment of inertia



Here's a render of the partially-CADed belt reduction


The other robot-arm progress I've made is translating my Python optimization script over to MATLAB.  This was an exercise in using MATLAB, a way to double check my work, a faster alternative to my Python script, and a way to generate better plots.  To check that my extremely simple Python ODE integrator worked, I compared the results it got with the results generated by integrating the equations of motion with ODE45 in MATLAB.

On the left is the MATLAB result, and on the right the Python result with step size decreasing by a factor of 10 each time.


And a comparison of the charts generated:


The MATLAB code can be found here.

October 12, 2013

A Really Really Fast Robot Arm

Since I started working in the cheetah robot lab, I've wanted to build a high-speed robot arm using a similar low-inertia, composite construction as used on the cheetah legs.

The arm will be roughly SCARA configuration.  Rather than having a motor directly at each joint, both the motors will be located at the shoulder of the arm, and the arms second link will be driven with a linkage.  This will make the arm light and low inertia, so it can be moved back and forth quickly.

I got some excellent motors for the arm from Charles at Swapfest.  They're a pair of ServoDisc Platinum UD9-E.  They have built in optical encoders, and because they're a real part rather than hobby grade equipment, they have an incredibly detailed spec sheet.  The ServoDiscs are coreless axial flux DC motors.  So basically a mini brushed etek.  Since the rotor is just copper windings, without any steel laminations, so there's zero cogging and very low rotor inertia, which make for extremely fast response.

To make the arm as fast as possible, I wrote some code to optimize the tip speed of the second segment of the arm over the gear ratio - arm length space.  Since I'm taking How To Matlab 2.086 this term, I took this opportunity to apply what I was learning in the class just write it in Python using ScyPi.

The main thing I 'd like to change about the code at this point is to implement one of the actual built-in ODE integrating functions.  Rather than figuring out how to use the stock ones I just used my own extremely simple fixed-timestep integrator.  It works fine, but it is pretty slow if you want good output resolution.

Since I know the basic construction method I would use for the arm as well as the motor specs, I was able to model the arm pretty easily.  The model arm is a 1.5" diameter carbon fiber tube I found the mass per length number for online, with a 70 gram mass at the end of it.  The arm is attached to an aluminum HTD timing belt pulley, which is described as an aluminum disc.  For a given arm length and pulley size, you can easily find the moment of inertia of the arm about its rotation point.  From there, you can use the torque-speed curve of the motor to get an expression for angular acceleration of the arm.

The code simulates the arm starting from standstill and applies constant voltage to the motor until a specified change in arm angle is reached (I've been using between 30 and 90 degrees).  Then the average speed of the tip of the arm over the motion can be found.  To get an idea of what ratios/lengths are optimal, you just repeat this process over a bunch of arm lengths and gear ratios.

Here's the Python code:


 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
from math import *
import scipy
from mpl_toolkits.mplot3d.axes3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np


def IntegrateArm(theta0, dtheta0, timestep, thetaEnd, L, Ta):
    
    time = 0
    theta = theta0
    dottheta = dtheta0    
    
    ########### Define Physical Properties ###########

    Kt = .081               #Motor torque constant in N-m/Amp
    V = 30.0                #Motor supply voltage
    R = .85                 #Motor terminal resistance in ohms
    L = L                   #Length of arm in m
    M = .07                 #Mass of end-effector in kg
    Ta = Ta                 #Arm pulley teeth
    Tm = 20.0               #Motor pulley teeth
    Rp = Ta*0.7958/1000     #Arm pulley radius in m, assuming HTD 5mm belt
    Ml = .164               #Linear density of arm in kg/m
    
    ########### Define Body Properties ###########
    Ma = L*Ml               #Mass of arm in kg
    Mp = 2700*.005*pi*Rp**2 #Approximate mass of pulley in kg (modeled as aluminum disc)
    Ia = (Ma*L**2)/3        #Moment of Inertia of Arm
    Im = M*L**2             #Moment of Inertia of end effector
    Ip = (Mp*Rp**2)/2       #Moment of inertia of pulley
    I = Ia + Im + Ip        #Total arm moment of inertia
    
    ########### Equation of Motion ###########
    tau = Kt*(V - Kt*dottheta*(Ta/Tm))/R   #Torque at motor shaft in N-m
    ddottheta = (tau*(Ta/Tm))/I


    while theta < thetaEnd:
        dottheta += ddottheta*timestep
        theta +=dottheta*timestep
        time += timestep
        tau = Kt*(V - Kt*dottheta*(Ta/Tm))/R   #Torque at motor shaft in N-m
        ddottheta = (tau*(Ta/Tm))/I
        
    V_avg = thetaEnd*L/time        
    return V_avg                                #Average arm tip velcity in m/s
        

def plotSurface(Lmin, Lmax, Tmin, Tmax, thetaInt):
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    x = np.linspace(Lmin, Lmax, 100)
    y = np.arange(Tmin, Tmax, 1.0)
    X, Y = np.meshgrid(x, y)
    z = np.zeros([len(x), len(y)])
    
    for L in range(len(x)):
        for T in range(len(y)):
            V = IntegrateArm(0, 0, 1e-4, thetaInt, x[L], y[T])
            z[L][T] = V
        

    ax.contour(X, Y, z.T)
    surf = ax.plot_surface(X, Y, z.T, rstride=1, cstride=1, cmap=cm.coolwarm,linewidth=0, antialiased=False)
    plt.show()


def findBestGear(L, Tmin, Tmax, thetaInt):
    T = np.arange(Tmin, Tmax, 1.0)
    V = []
    for i in range(len(T)):
        val =IntegrateArm(0, 0, 1e-4, thetaInt, L, T[i])
        V.append(val)
    return t[V.index(max(V))]


The plotSurface function generates a nice 3D picture of the tip velocity vs number of pulley teeth on the arm and arm length:

Unfortunately there doesn't seem to be any global maximum.  If you keep increasing gear ratio and arm length, average tip-speed keeps on increasing.  However, for a given arm length and arm swing angle, there is clearly an optimal gear ratio.  Once I start actually designing parts, I can refine the model with more accurate arm inertias, but this gives me a good starting point.

Finally, I contacted NAC Harmonic, and they're sending me (for free!)  a 50:1 harmonic drive component set, which may or may not be used in the arm.