Deltaprinter python script

#!/usr/bin/python

import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import copy
import scipy.io
import scipy.interpolate
from matplotlib.mlab import griddata

#not necessarily radially symmetric, but rod lengths still maintain parallel sides
class DeltaPrinter(object):
    L = 270     #rod lengths, kept constant to maintain pantograph constraints
    Offset_e = 36.27        #assuming round and constant effector offsets in Y only
    Offset_c = 38       #assuming constant carriage offsets in Y only
    R = 199.5           #assuming constant column radii

    #vectors from center of effector to attachment points (local view, y-axis outwards from effector towards carriage)
    Ae = np.array([0,0])
    Be = np.array([0,0])
    Ce = np.array([0,0])

    #carriage offsets from column to arm attachment (local view, y-axis towards effector, z-axis upwards) - should only be adjusted for error analysis purposes??
    Ac = np.array([0,0])
    Bc = np.array([0,0])
    Cc = np.array([0,0])

    Hz = 0      #offset of nozzle end from effector plane (just additional vertical offset)

    #column radii from center of build plate - flexibility for independent assignment
    Ar = 180;
    Br = 180;
    Cr = 180;

    #column angle offset, shouldn't be adjusted (ever?)
    Aa = 0
    Ba = 2*np.pi/3
    Ca = 4*np.pi/3

    #virtual pivot locations - not set yet, calculated during ik setup
    Av = None
    Bv = None
    Cv = None
    dCol = None                 #combination of virtual pivots for easier indexing

    def __init__(self):
        self.R = 199.5          #will trigger extra variable assignments via __setattr__
        self.Offset_e = 36.27
        self.Offset_c = 38
        self._updatePrinter()   #will set the virtual pivot locations

    #helper function used to debug issues w/ fk/ik
    def _rotateTowers(self,ang):
        print "Towers at angles "+repr(np.array([self.Aa,self.Ba,self.Ca]))
        self.Aa += ang
        self.Ba += ang
        self.Ca += ang
        self._updatePrinter()
        print "Towers now at angles "+repr(np.array([self.Aa,self.Ba,self.Ca]))

    #setter functions will make additional call to _updatePrinter() to re-generate virtual positions
        #dangerous! need to watch out for loops in callback
    def __setattr__(self,name,value):
        if name=='Offset_e':
            self.Ae=np.array([0.,value])
            self.Be=np.array([0.,value])
            self.Ce=np.array([0.,value])
        elif name=='Offset_c':
            self.Ac=np.array([0.,value])
            self.Bc=np.array([0.,value])
            self.Cc=np.array([0.,value])
        elif name=='R':
            self.Ar=value
            self.Br=value
            self.Cr=value

        super(DeltaPrinter,self).__setattr__(name,value)    #still make set changes as normal to bulk variables

    #calculate pivot and virtual positions by reducing system to point intersection >> changes system definition
        #private function to make ik/fk calculations faster >> only called if system parameters changed
    def _updatePrinter(self):
        #rotation matrices for each column (offsets given per local frames)
        Ra = np.matrix([[np.cos(np.pi/2-self.Aa), -np.sin(np.pi/2-self.Aa)],[np.sin(np.pi/2-self.Aa), np.cos(np.pi/2-self.Aa)]])
        Rb = np.matrix([[np.cos(np.pi/2-self.Ba), -np.sin(np.pi/2-self.Ba)],[np.sin(np.pi/2-self.Ba), np.cos(np.pi/2-self.Ba)]])
        Rc = np.matrix([[np.cos(np.pi/2-self.Ca), -np.sin(np.pi/2-self.Ca)],[np.sin(np.pi/2-self.Ca), np.cos(np.pi/2-self.Ca)]])

        #column locations (x,y in global frame)
        A = np.array([self.Ar*np.cos(self.Aa),self.Ar*np.sin(self.Aa)])
        B = np.array([self.Br*np.cos(self.Ba),self.Br*np.sin(self.Ba)])
        C = np.array([self.Cr*np.cos(self.Ca),self.Cr*np.sin(self.Ca)])

        #calculate column pivot locations, applying carriage offsets (global frame)
        Ap = A-self.Ac*Ra
        Bp = B-self.Bc*Rb
        Cp = C-self.Cc*Rc

        #calculate virtual pivot locations, applying effector offsets
        self.Av = Ap-self.Ae*Ra
        self.Bv = Bp-self.Be*Rb
        self.Cv = Cp-self.Ce*Rc
        self.dCol = np.vstack((self.Av,self.Bv,self.Cv));   #combines virtual tower locations for better indexing

    def getR(self):
        R_min = np.array([self.Ar,self.Br,self.Cr]).min()
        R_max = np.array([self.Ar,self.Br,self.Cr]).max()
        if R_min==R_max:
            self.R = R_max
        return np.array([R_min,R_max])

    #forward kinematics: takes in control heights for each column
        #via circumcenter approach that apparently doesn't always work
    def fk2(self,Az,Bz,Cz):
        self._updatePrinter()

        #calculate circumcenter from projected virtual triangle
        Pa = np.array([self.Av[0,0], self.Av[0,1], Az])
        Pb = np.array([self.Bv[0,0], self.Bv[0,1], Bz])
        Pc = np.array([self.Cv[0,0], self.Cv[0,1], Cz])

        alpha = (np.power(np.linalg.norm(Pb-Pc),2)*np.vdot(Pa-Pb,Pa-Pc)) / (2*np.power(np.linalg.norm(np.cross(Pa-Pb,Pb-Pc)),2))
        beta = (np.power(np.linalg.norm(Pa-Pc),2)*np.vdot(Pb-Pa,Pb-Pc)) / (2*np.power(np.linalg.norm(np.cross(Pa-Pb,Pb-Pc)),2))
        gamma = (np.power(np.linalg.norm(Pa-Pb),2)*np.vdot(Pc-Pa,Pc-Pb)) / (2*np.power(np.linalg.norm(np.cross(Pa-Pb,Pb-Pc)),2))
        r = (np.linalg.norm(Pa-Pb) * np.linalg.norm(Pb-Pc) * np.linalg.norm(Pc-Pa)) / (2 * np.linalg.norm(np.cross(Pa-Pb,Pb-Pc)))

        P = alpha*Pa + beta*Pb + gamma*Pc
        N = np.cross(Pa-Pc,Pb-Pa)
        nn = np.sqrt(np.power(self.L,2)-np.power(r,2))  #normal length from virtual carriage triangle to effector point

        E = P+N/np.linalg.norm(N) * nn
        return E

    #using trilateration as suggested by Steve Graves document
        #uses virtual tower placements and rod lengths (avoids effector size)
    def fk(self,Az,Bz,Cz):
        dZ = np.array([Az,Bz,Cz])
        dColP = np.zeros([3,3])
        for i in xrange(3):
            dColP[i][0] = self.dCol[i,0]
            dColP[i][1] = self.dCol[i,1]
            dColP[i][2] = dZ[i]

        #using same notation as Steve Graves example:
        p12 = dColP[1] - dColP[0]
        d = np.linalg.norm(p12)
        ex = p12 / d

        p13 = dColP[2] - dColP[0]
        i = np.dot(ex,p13)
        iex = i*ex

        ey = p13 - iex
        j = np.linalg.norm(ey)
        ey = ey / j

        ez = np.cross(ex,ey)

        Xnew = d/2
        Ynew = ((i*i + j*j)/2 - i*Xnew)/j
        Znew = np.sqrt(self.L*self.L - Xnew*Xnew - Ynew*Ynew)

        cartesian = dColP[0] + (Xnew * ex) + (Ynew * ey) + (-Znew * ez)
        return cartesian

    #_updatePrinter() need to be called prior to ik if system parameters have been changed
    def ik(self,X,Y,Z):
        #perform quick check to ensure that (x,y,z) is within valid envelope:
        R_max = self.getR()[1]
        if np.linalg.norm(np.array([X,Y])) > R_max or Z<0:
            #print "ERROR: x,y,z beyond valid envelope"
            return np.array([-1,-1,-1])

        Acz = np.sqrt(np.power(self.L,2) - np.power(X-self.Av[0,0],2) - np.power(Y-self.Av[0,1],2))
        Bcz = np.sqrt(np.power(self.L,2) - np.power(X-self.Bv[0,0],2) - np.power(Y-self.Bv[0,1],2))
        Ccz = np.sqrt(np.power(self.L,2) - np.power(X-self.Cv[0,0],2) - np.power(Y-self.Cv[0,1],2))

        Az = Z+Acz+self.Hz
        Bz = Z+Bcz+self.Hz
        Cz = Z+Ccz+self.Hz
        return np.array([Az,Bz,Cz])

    #checks that the control inputs are all valid
    def _isValid(self,abc):
        return (abc>0).all()

    #randomize set of points and compare ik w/ fk
        #don't particularly care about z-sampling, as offsets in ik/fk are trivial
        #if issues arise here for any sampling, we are in trouble
    def _validate(self,N=10):
        self._updatePrinter()
        z = 0       #arbitrary z value selection

        R_min = self.getR()[0]
        err_sum = 0.
        for i in xrange(N):
            r = np.random.rand() * R_min
            ang = np.random.rand() * np.pi * 2.0
            x = r*np.cos(ang)
            y = r*np.sin(ang)

            ABC = self.ik(x,y,z)
            XYZ = self.fk(ABC[0],ABC[1],ABC[2])

            err = np.linalg.norm(XYZ-np.array([x,y,z]))
            err_sum += err
            print "Iteration "+repr(i+1)+": error "+repr(err)
            print "Initial XYZ: "+repr(np.array([x,y,z]))
            print "Column vals: "+repr(ABC)
            print "Final XYZ: "+repr(XYZ)+"\n"

        print "Random Test Average error: "+repr(err_sum/N)+"\n"

        #perform additional rotational test w/ respect to towers to find potential errors:
        tower_ang = np.array([self.Aa,self.Ba,self.Ca])
        rot_err_sum = 0.
        for i in xrange(N):
            r = np.random.rand() * R_min
            tower_abc = np.zeros([tower_ang.size,3])
            for j in xrange(tower_ang.size):
                ang = tower_ang[j]
                x = r*np.cos(ang)
                y = r*np.sin(ang)
                tower_abc[j] = self.ik(x,y,z)
            tower_abc_sum = tower_abc.sum(0)
            print "Rotational test sum for radius "+repr(r)+": "+repr(tower_abc_sum)

    #determines how much control values should be modified for a given step change in cartesian space
    #evaluated at (x,y,z) for a workspace step dx averaged over N number of evenly sampled radial directions
    def evaluateLocalStep(self,x,y,z=0,dx=1,N=12):
        abc = self.ik(x,y,z)
        dabc = np.zeros(3)
        dn = 0
        if self._isValid(abc):
            ang_step = 2*np.pi/N
            for ni in xrange(N):
                ang = ni*ang_step
                dxi = dx * np.cos(ang)
                dyi = dx * np.sin(ang)

                abc2 = self.ik(x+dxi,y+dyi,z)
                if self._isValid(abc2):
                    dabc = dabc+abs(abc-abc2)
                    dn += 1
            if dn>0:
                dabc = dabc/dn
        return dabc

    #N: angular intervals to sample (12 to accommodate 4 cartesian neighbors and 3 towers)
    #dx:
    def evaluateStep(self,points=None,dx=1,N=12,plot=False):
        ang_step = 2*np.pi/N

        z = 0
        R_min = self.getR()[0]

        if points is None:
            points = genGrid(R_min)

        res = np.zeros([points.shape[0],6]) #[x,y,r,avg_da,avg_db,avg_dc]
        ri = 0
        for i in xrange(points.shape[0]):
            x = points[i][0]
            y = points[i][1]

            dabc = self.evaluateLocalStep(x,y,z,dx,N)

            if dabc.any():
                res[ri] = np.array([x,y,np.linalg.norm([x,y]),dabc[0],dabc[1],dabc[2]])
                ri+=1

        res = res[0:ri,:]   #trim the result array

        if plot:
            #optional plotting for step sizes:

            #3 subplots for individual tower resolution requirements:
            fig1 = plt.figure(1)

            plt.subplot(131)
            cs11 = plotContourf(res[:,0],res[:,1],res[:,3])
            plt.title('Tower A')

            plt.subplot(132)
            cs12 = plotContourf(res[:,0],res[:,1],res[:,4])
            plt.title('Tower B')

            plt.subplot(133)
            cs13 = plotContourf(res[:,0],res[:,1],res[:,5])
            plt.title('Tower C')

            fig1.subplots_adjust(bottom=0.2)
            cbar1_ax = fig1.add_axes([0.05, 0.05, 0.9, 0.05])
            fig1.colorbar(cs13,cax=cbar1_ax,orientation='horizontal')

            fig2 = plt.figure(2)

            plt.subplot(141)
            t_min = np.amin(res[:,3:6],axis=1)
            cs21 = plotContourf(res[:,0],res[:,1],t_min)
            fig2.colorbar(cs21)
            plt.title('Min Tower Resolution')

            plt.subplot(142)
            t_max = np.amax(res[:,3:6],axis=1)
            cs22 = plotContourf(res[:,0],res[:,1],t_max)
            fig2.colorbar(cs22)
            plt.title('Max Tower Resolution')

            plt.subplot(143)
            t_mean = np.mean(res[:,3:6],axis=1)
            cs23 = plotContourf(res[:,0],res[:,1],t_mean)
            fig2.colorbar(cs23)
            plt.title('Mean Tower Resolution')

            plt.subplot(144)
            t_std = np.std(res[:,3:6],axis=1)
            cs24 = plotContourf(res[:,0],res[:,1],t_std)
            fig2.colorbar(cs24)
            plt.title('Tower Std Deviation')

            fig1.tight_layout()
            fig2.tight_layout()

            plt.show()
        return res

    #determines the top curvature of the workspace that is achievable (essentially details the meniscus that forms)
    #low points of meniscus found where arms are entirely vertical
    def evaluateShape(self,points=None,plot=False):
        if points is None:
            points = genGrid(self.getR()[0])
        z = 0
        ri = 0
        points = np.vstack((np.array([0,0]),points))    #initial seed is always center point
        res = np.zeros([points.shape[0],11])
        for i in xrange(points.shape[0]):
            x = points[i][0]
            y = points[i][1]

            abc = self.ik(x,y,z)    #tower heights
            abc_elevation = np.zeros(3)
            abc_azimuth = np.zeros(3)
            #calculate effector angles (projected on rz, xz planes)
            for j in xrange(3): #virtual tower positions found in dCol
                L_xy = np.linalg.norm(np.array([x,y])-np.array([self.dCol[j,0],self.dCol[j,1]]))
                L_z = abc[j]-(z+self.Hz)    #z specifies nozzle point
                abc_elevation[j] = np.arctan2(L_z,L_xy)
                abc_azimuth[j] = np.arctan2(self.dCol[j,1]-y,self.dCol[j,0]-x)

            if self._isValid(abc):
                res[ri] = np.array([x,y,abc[0],abc[1],abc[2],abc_elevation[0],abc_elevation[1],abc_elevation[2],abc_azimuth[0],abc_azimuth[1],abc_azimuth[2]])  #every index should have valid solutions
                ri+=1

        res = res[0:ri,:]

        if plot:
            fig1 = plt.figure(1)
            t_max = np.amax(res[:,2:5],axis=1)
            cs = plotContourf(res[:,0],res[:,1],t_max)
            fig1.colorbar(cs)
            plt.title('Max Tower Position')

            fig2 = plt.figure(2)
            theta_elevation = res[:,5:8]-res[0,5:8] #relative to initial center seed point
            theta_azimuth = res[:,8:11]-res[0,8:11]

            plt.subplot(231)
            cs_e0 = plotContourf(res[:,0],res[:,1],theta_elevation[:,0])
            fig2.colorbar(cs_e0)
            plt.title('Elevation A')
            plt.subplot(232)
            cs_e1 = plotContourf(res[:,0],res[:,1],theta_elevation[:,1])
            fig2.colorbar(cs_e1)
            plt.title('Elevation B')
            plt.subplot(233)
            cs_e2 = plotContourf(res[:,0],res[:,1],theta_elevation[:,2])
            fig2.colorbar(cs_e2)
            plt.title('Elevation C')

            plt.subplot(234)
            cs_a0 = plotContourf(res[:,0],res[:,1],theta_azimuth[:,0])
            fig2.colorbar(cs_a0)
            plt.title('Azimuth A')
            plt.subplot(235)
            cs_a1 = plotContourf(res[:,0],res[:,1],theta_azimuth[:,1])
            fig2.colorbar(cs_a1)
            plt.title('Azimuth B')
            plt.subplot(236)
            cs_a2 = plotContourf(res[:,0],res[:,1],theta_azimuth[:,2])
            fig2.colorbar(cs_a2)
            plt.title('Azimuth C')

            plt.show()

        return res

    #returns basic, summarized results for a given delta design
    def eval(self,points=None):
        if points is None:
            points = genGrid(self.getR()[0])
            #for comparisons, might want to feed in

        res_shape = self.evaluateShape(points)
        #find amount of vertical space that is lost for the given workspace points (should be capped at arm length value)
        #tower_max = np.amax(res_shape[:,2:5])  #max tower position
        tower_max = np.zeros(3)
        tower_min = np.zeros(3)
        travel_max = np.zeros(3)                #determine max amount of tower travel from center position (relative positioning) - no need for travel_min (just 0,0,0 for center)
        elevation_max = np.zeros(3)
        elevation_min = np.zeros(3)
        azimuth_max = np.zeros(3)
        azimuth_min = np.zeros(3)
        elevation_range = np.zeros(3)
        azimuth_range = np.zeros(3)

        print "\n"
        print "-- Delta Shape Summary --"
        for i in xrange(3):
            tower_max[i] = res_shape[:,2+i].max()
            tower_min[i] = res_shape[:,2+i].min()
            tower_travel = res_shape[:,2+i]-res_shape[0,2+i]    #relative to center position for current tower
            travel_max[i] = tower_travel.max()
            elevation_max[i] = res_shape[:,5+i].max()
            elevation_min[i] = res_shape[:,5+i].min()
            azimuth_max[i] = res_shape[:,8+i].max()
            azimuth_min[i] = res_shape[:,8+i].min()
            elevation_range[i] = elevation_max[i]-elevation_min[i]
            azimuth_range[i] = azimuth_max[i]-azimuth_min[i]

        print "Max Tower Position: "+repr(tower_max)
        print "Min Tower Position: "+repr(tower_min)
        print "Max Tower Travel from Center: "+repr(travel_max)

        res_step = self.evaluateStep(points)
        #used to determine required resolution of tower motion for step motion in cartesian workspace
        res_min = np.zeros(3)
        res_max = np.zeros(3)
        res_mean = np.zeros(3)
        res_std = np.zeros(3)
        for i in xrange(3):
            res_min[i] = res_step[:,3+i].min()
            res_max[i] = res_step[:,3+i].max()
            res_mean[i] = res_step[:,3+i].mean()
            res_std[i] = res_step[:,3+i].std()

        print "\n"
        print "-- Delta Motion for Step Cartesian Move --"
        print "Min Tower Motion: "+repr(res_min)
        print "Max Tower Motion: "+repr(res_max)
        print "Avg Tower Motion: "+repr(res_mean)
        print "Tower Motion STD: "+repr(res_std)

        res = np.vstack((tower_max,tower_min,travel_max,elevation_max,elevation_min,elevation_range,azimuth_max,azimuth_min,azimuth_range,res_min,res_max,res_mean,res_std))
        return res


#will use error values/designations similar to what is found in Repetier firmware
class DeltaError(object):
    delta0 = None
    delta1 = None

    dTower = np.array([0.,0.,0.])   #related to z-limit errors for each tower - used to introduce tower noise, poor zero-ing
    dL = 0.

    #ASSEMBLY ERRORS:
    dOffset_e = 0.
    dOffset_c = 0.

    #effector offset error
    dAe = np.array([0,0])
    dBe = np.array([0,0])
    dCe = np.array([0,0])

    #carriage offset error
    dAc = np.array([0,0])
    dBc = np.array([0,0])
    dCc = np.array([0,0])

    #BASE STRUCTURE ERRORS:
    #radii of base towers - related to setting PRINTER_RADIUS, DELTA_RADIUS_CORRECTION
    dAr = 0.
    dBr = 0.
    dCr = 0.

    #angular offset of base towers - related to DELTA_ALPHA_[A-C]
    dAa = 0.
    dBa = 0.
    dCa = 0.

    def __init__(self,delta):
        self.delta0 = copy.copy(delta)      #control delta
        self.delta1 = copy.copy(delta)      #error delta - initially same as source

    def __setattr__(self,name,value):
        rname = name[1:]    #remove possible 'd' prefix
        if hasattr(self.delta0,rname):
            setattr(self.delta1, rname, getattr(self.delta0,rname)+value)
            print "Control Delta "+rname+": "+repr(getattr(self.delta0,rname))
            print "Error Delta "+rname+": "+repr(getattr(self.delta1,rname))
            self.delta1._updatePrinter()    #update virtual tower model internally

        super(DeltaError,self).__setattr__(name,value)

    #returns baseline path and corresponding path in error model
    def eval(self,path=None,plot=False):
        if path is None:
            path = genGrid(self.delta0.getR()[0])
        z = 0
        ri = 0
        res = np.zeros([path.shape[0],5])
        for i in xrange(path.shape[0]):
            x = path[i][0]
            y = path[i][1]
            abc = self.delta0.ik(x,y,z)
            if self.delta0._isValid(abc):
                xyz = self.delta1.fk(abc[0]+self.dTower[0],abc[1]+self.dTower[1],abc[2]+self.dTower[2])     #implement tower offsets here
                res[ri] = np.array([x,y,xyz[0]-x,xyz[1]-y,xyz[2]-z])
                ri += 1
        res = res[0:ri,:]

        if plot:
            fig1 = plt.figure(1)
            plt.subplot(131)    #x discrepancy
            cs1 = plotContourf(res[:,0],res[:,1],res[:,2])
            fig1.colorbar(cs1)
            plt.title('x error')

            plt.subplot(132)    #y discrepancy
            cs2 = plotContourf(res[:,0],res[:,1],res[:,3])
            fig1.colorbar(cs2)
            plt.title('y error')

            plt.subplot(133)    #z discrepancy
            cs3 = plotContourf(res[:,0],res[:,1],res[:,4])
            fig1.colorbar(cs3)
            plt.title('z error')

            fig1.tight_layout()

            fig2 = plt.figure(2)
            r = np.sqrt(res[:,0]*res[:,0]+res[:,1]*res[:,1])
            r_err = np.sqrt(res[:,2]*res[:,2]+res[:,3]*res[:,3])

            ri = np.nonzero(r)      #removing the 0 radii components
            r_err = r_err[ri]
            r = r[ri]
            r_ratio = r_err/r

            plt.subplot(121)    #r discrepancy w/ respect to distance from center
            fit1 = np.polyfit(r,r_err,1)
            fit1_fn = np.poly1d(fit1)
            plt.plot(r,r_err,'.k')
            plt.plot(r,fit1_fn(r),'-r')
            plt.title('Error')
            plt.xlabel('Radius')
            plt.ylabel('Error')

            plt.subplot(122)    #ratio of r discrepancy to r
            fit2 = np.polyfit(r,r_ratio,1)
            plt.plot(r,r_ratio,'.k')
            fit2_fn = np.poly1d(fit2)
            plt.plot(r,fit2_fn(r),'-r')
            plt.title('Error/Radius')
            plt.ylabel('Error/Radius')
            plt.xlabel('Radius')

            fig2.tight_layout()

            plt.show()

        return res

#HELPER FUNCTIONS:
def plotContourf(x,y,z,contour_n=10,xyz_step=1):
    xi = np.arange(x.min(),x.max(),xyz_step)
    yi = np.arange(y.min(),y.max(),xyz_step)
    zi = griddata(x,y,z,xi,yi,interp='linear')
    cs = plt.contourf(xi,yi,zi,contour_n,cmap=plt.cm.gray)
    plt.xlim(x.min()-10,x.max()+10)     #generate some gap along the edges for viewing
    plt.ylim(y.min()-10,y.max()+10)
    plt.xlabel('x')
    plt.ylabel('y')
    plt.axis('equal')
    return cs

#generates a 2d, x-y path for a spiral path
    #- r: max radius of spiral
    #- nloop: number of loops in spiral
    #- n: number of points
def genSpiral(r=100,nloop=1,n=25):
    rs = np.linspace(0,r,n)
    ts = np.linspace(0,nloop*np.pi*2,n)

    x = rs*np.cos(ts)
    y = rs*np.sin(ts)

    return np.vstack((x,y)).transpose()

#takes delta parameters to generate voxelized grid w/ given sampling step
#note that this generates a cartesian grid not necessarily centered in radial build space, so evaluation functions may not show symmetric results depending on sampling
def genGrid(R,xy_step=5):
    xx = np.arange(-R,R+xy_step,xy_step)
    yy = np.arange(-R,R+xy_step,xy_step)

    g = np.zeros([xx.size*yy.size,2])   #we only care about xy grid making (z is usually constant in our tests)
    ri = 0                                      #index-ing for results array
    for xi in xrange(xx.size):
        for yi in xrange(yy.size):
            if np.linalg.norm(np.array([xx[xi],yy[yi]])) > R:           #invalid or nonexistent ik result
                continue
            else:
                g[ri] = np.array([xx[xi],yy[yi]])
                ri+=1

    print "Grid generated w/ x ["+repr(g[:,0].min())+", "+repr(g[:,0].max())+"], y ["+repr(g[:,1].min())+", "+repr(g[:,1].max())+"]"
    return g[0:ri,:]

if __name__=="__main__":
    font = {'size':9}
    matplotlib.rc('font',**font)

    d = DeltaPrinter()
    de = DeltaError(d)