leastSquares.py

import sys
import math as m
import numpy as np
#from mpl_toolkits.mplot3d import Axes3D
#import matplotlib.pyplot as plt
#import scipy.optimize
#import functools

#theta1s=[]
#theta2s=[]

#NOT USED IN CURRENT METHODOLOGY
# use all the time
def calcx(l1, l2, theta1, theta2):
    return (l1*m.cos(theta1) + l2*m.cos(theta2))

#NOT USED IN CURRENT METHODOLOGY
# use to calculate touch probe stuff?
def calcy(l1, l2, theta1, theta2):
    return (l1*m.sin(theta1) + l2*m.sin(theta2))

#NOT USED IN CURRENT METHODOLOGY
#in params, slope is first term (c) and y intercept is second term (d)
def complicatedline(l1, l2, theta1, theta2, params):
    c = params[0]
    d = params[1]
    y = c*(calcx(l1, l2, theta1, theta2)) + d
    return y

# SWITCHED STRATEGIES, now we are given C and D

#constructs matrix A for solving least squares
def constructmatA(C, theta1s, theta2s):
    cfloat = float(C)
    a = []
    for i in range(len(theta1s)):
        #print theta1s[i]
        theta1 = float(theta1s[i])
        theta2 = float(theta2s[i])
        a.append([cfloat*m.cos(theta1)-m.sin(theta1), cfloat*m.cos(theta2)-m.sin(theta2)])
    return a
    
def constructmatb(D, theta1s):
    dfloat = float(D)
    b = []
    for theta in theta1s:
        b.append([-dfloat])
    return b

theta1sString = sys.argv[1]
theta2sString = sys.argv[2]
cString = sys.argv[3]
dString = sys.argv[4]

theta1s = theta1sString.split(',')
theta2s = theta2sString.split(',')
c = float(cString)
d = float(dString)

A = constructmatA(c, theta1s, theta2s)
b = constructmatb(d, theta1s)

fullRes = np.linalg.lstsq(A,b, rcond=None)

lengths = [fullRes[0][0][0], fullRes[0][1][0]]

print lengths
sys.stdout.flush()

'''
#test case

#theta1s=[m.pi/2, m.pi*50/180, m.pi*25/180]
#theta2s=[0, m.pi*10/180, m.pi*60/180]
theta1s=[m.pi*75/180, m.pi*60/180, m.pi*45/180]
theta2s=[m.pi*-30/180, m.pi*5/180, m.pi*15/180]

#test case with c = 0, d = 0 so x = 0 I DO NOT RECOMMEND USING C = 0 AND D = 0, i think information is lost this way
#current test case: c = 1, d = -2
testA = constructmatA(1, theta1s, theta2s)
testb = constructmatb(-2, theta1s)

np.linalg.lstsq(testA, testb)
'''