diff --git a/modules/robot/forwardTransform.js b/modules/robot/forwardTransform.js new file mode 100644 index 0000000000000000000000000000000000000000..0c6539f3affd91c1c04702aeb672c7659296738f --- /dev/null +++ b/modules/robot/forwardTransform.js @@ -0,0 +1,166 @@ +// boilerplate atkapi header +const JSUnit = require('../../src/jsunit.js') +const MJS = require('mathjs') + +let Input = JSUnit.Input +let Output = JSUnit.Output +let State = JSUnit.State + +function forwardTransform() { + var theta1 = 0 + var theta2 = 0 + var l1 = 0 + var l2 = 0 + + var forwardTransform = { + description: { + name: 'forwardTransform Parser', + alt: 'line of forwardTransform -> points' + } + } + + // log more + var verbose = false + + // one caveat here is that we can't dynamically add objects to this, + // or they don't get getter / settered when we do + forwardTransform.state = State() + // forwardTransform.state.mode = 'G0' + // forwardTransform.state.G0 = 1200 + // forwardTransform.state.G1 = 400 + + forwardTransform.inputs = { + theta1: Input('any', intakeTheta1), + theta2: Input('any', intakeTheta2), + l1: Input('any', intakeL1), + l2: Input('any', intakeL2) + } + + forwardTransform.outputs = { + originpt: Output('tuple?'), + jointpt: Output('tuple?'), + touchpt: Output('tuple?') + // instructionOut: Output('move instruction'), + // modeChange: Output('string') + } + + function intakeTheta1(num){ + theta1 = num + var points = parseforwardTransform() + forwardTransform.outputs.originpt.emit(points.originpt) + forwardTransform.outputs.jointpt.emit(points.jointpt) + forwardTransform.outputs.touchpt.emit(points.touchpt) + + } + + function intakeTheta2(num){ + theta2 = num + var points = parseforwardTransform() + forwardTransform.outputs.originpt.emit(points.originpt) + forwardTransform.outputs.jointpt.emit(points.jointpt) + forwardTransform.outputs.touchpt.emit(points.touchpt) + //console.log('theta2 input') + } + + function intakeL1(num){ + l1 = num + var points = parseforwardTransform() + forwardTransform.outputs.originpt.emit(points.originpt) + forwardTransform.outputs.jointpt.emit(points.jointpt) + forwardTransform.outputs.touchpt.emit(points.touchpt) + //console.log('l1 input') + } + + function intakeL2(num){ + l2 = num + var points = parseforwardTransform() + forwardTransform.outputs.originpt.emit(points.originpt) + forwardTransform.outputs.jointpt.emit(points.jointpt) + forwardTransform.outputs.touchpt.emit(points.touchpt) + //console.log('l2 input') + } + + // input functions + /* + function onLineIn(str){ + var instruction = parseforwardTransform(str) + if (instruction.hasMove) { + if(verbose) console.log('forwardTransform:', instruction) + forwardTransform.outputs.instructionOut.emit(instruction) + } else { + if(verbose) console.log('forwardTransform:', forwardTransform.state.mode) + forwardTransform.outputs.modeChange.emit(forwardTransform.state.mode) + } + } + */ + + /* + // local functions + function getKeyValues(str) { + var kv = {} + for (var i = 0; i < str.length; i++) { + if (str[i].match('[A-Za-z]')) { // regex to match upper case letters + var lastIndex = str.indexOf(' ', i) + if (lastIndex < 0) { + lastIndex = str.length + } + var key = str[i].toUpperCase() + kv[key] = parseFloat(str.slice(i + 1, lastIndex)) + } + } + return kv + } + */ + + // TODO: test, can we link global vars to ui objects ... + // forwardTransform.ui.mode.value = var ? no bc set / get etc + // more like var = forwardTransform.ui.mode.value ? is this referential? + + function parseforwardTransform() { + var points = { + originpt: (0,0), + jointpt: (0,0), + touchpt: (0,0) + } + + points.jointpt = (l1*Math.cos(theta1), l1*Math.sin(theta1)) + points.touchpt = (l1*Math.cos(theta1)+l2*Math.cos(theta2), l1*Math.sin(theta1)+l2*Math.sin(theta2)) + /* var instruction = { + position: {}, + hasMove: false, + speed: 0 + } + + kv = getKeyValues(str) + // track modality + if (kv.G == 0 | kv.G == 1) { + forwardTransform.state.mode = 'G' + kv.G.toString() + } else if (kv.G != null) { + // no arcs pls + console.log('unfriendly forwardTransform mode!', kv) + } + + for (key in kv) { + if (key.match('[A-EX-Z]')) { + instruction.position[key] = kv[key] + instruction.hasMove = true + } else if (key.match('[F]')) { + // tricky / ugly: sets using the mode state string as object key + forwardTransform.state[forwardTransform.state.mode] = kv.F + } + } + + instruction.speed = forwardTransform.state[forwardTransform.state.mode] + // and this for help later? + instruction.kv = kv + */ + return points + } + + return forwardTransform + +} + + +// export the module +module.exports = forwardTransform \ No newline at end of file diff --git a/modules/robot/leastSquares.js b/modules/robot/leastSquares.js new file mode 100644 index 0000000000000000000000000000000000000000..2f2c0224992d5fdadd2aaa0a6ef0d9a96491d69d --- /dev/null +++ b/modules/robot/leastSquares.js @@ -0,0 +1,147 @@ +/* Easiest way I know of is to use "child_process" package which comes packaged with node. + +Then you can do something like: */ +const JSUnit = require('../../src/jsunit.js') +const MJS = require('mathjs') +const spawn = require("child_process").spawn +//const express = require('express') +//const app = express() + + +let Input = JSUnit.Input +let Output = JSUnit.Output +let State = JSUnit.State + +function leastSquares() { + var theta1s = 0 + var theta2s = 0 + + var leastSquares = { + description: { + name: 'leastSquares Parser', + alt: 'line of leastSquares -> l1 and l2 values' + } + } + + // log more + // var verbose = false + + // one caveat here is that we can't dynamically add objects to this, + // or they don't get getter / settered when we do + leastSquares.state = State() + leastSquares.state.c = 1 + leastSquares.state.d = -2 + // leastSquares.state.G1 = 400 + + leastSquares.inputs = { + theta1s: Input('any', intakeTheta1s), // can be taken in as a string + theta2s: Input('any', intakeTheta2s) + } + + leastSquares.outputs = { + l1: Output('num'), + l2: Output('num') + // instructionOut: Output('move instruction'), + // modeChange: Output('string') + } + + function intakeTheta1s(list){ + theta1s = list + var lengths = parseleastSquares() + } + + function intakeTheta2s(list){ + theta2s = list + var lengths = parseleastSquares() + //console.log('theta2 input') + } + + + function thetasToStrings(list){ + + } + + function parseleastSquares() { + var lengths = { + //l1: 0, + //l2: 0 + test: 'something' + } + + theta1sString = theta1s.toString() + theta2sString = theta2s.toString() + cString = leastSquares.state.c.toString() + dString = leastSquares.state.d.toString() + //console.log("we here at least:"+theta1sString) + + const pythonProcess = spawn('python',['py/leastSquares.py', theta1sString, theta2sString, cString, dString]) + //console.log('do we get here?') + //console.log(pythonProcess) + + pythonProcess.stdout.on('data', (data) => { + console.log('got here yay python!!') + console.log(data.toString()) + lengths.test = data.toString() + leastSquares.outputs.l1.emit(lengths.test) + leastSquares.outputs.l2.emit(lengths.test) + }) + + pythonProcess.stderr.on('data', function(data){ + console.log("Error: " + data); + }); + + /* + app.get('/', (req, res) => { + const { spawn } = require('child_process'); + const pythonProcess = spawn('python',['leastSquares.py', theta1s, theta2s, c, d]); + + pythonProcess.stdout.on('data', function(data) { + + console.log(data.toString()); + res.write(data); + res.end('end'); + }); + }) + + app.listen(4000, () => console.log('Application listening on port 4000!')) + */ + + /* var instruction = { + position: {}, + hasMove: false, + speed: 0 + } + + kv = getKeyValues(str) + // track modality + if (kv.G == 0 | kv.G == 1) { + leastSquares.state.mode = 'G' + kv.G.toString() + } else if (kv.G != null) { + // no arcs pls + console.log('unfriendly leastSquares mode!', kv) + } + + for (key in kv) { + if (key.match('[A-EX-Z]')) { + instruction.position[key] = kv[key] + instruction.hasMove = true + } else if (key.match('[F]')) { + // tricky / ugly: sets using the mode state string as object key + leastSquares.state[leastSquares.state.mode] = kv.F + } + } + + instruction.speed = leastSquares.state[leastSquares.state.mode] + // and this for help later? + instruction.kv = kv + */ + return lengths + } + + return leastSquares + +} + + +// export the module +module.exports = leastSquares \ No newline at end of file diff --git a/modules/ui/string.js b/modules/ui/string.js new file mode 100644 index 0000000000000000000000000000000000000000..1b47b82cf2258385110278c099314d322775dace --- /dev/null +++ b/modules/ui/string.js @@ -0,0 +1,70 @@ +// boilerplate rndmc header +const JSUnit = require('../../src/jsunit.js') +let Input = JSUnit.Input +let Output = JSUnit.Output +let State = JSUnit.State + +// interface elements +const JSUI = require('../../src/jsui.js') +let UI = JSUI.UI + +// a constructor, a fn, a javascript mess +function uiString() { + + // this is the tiny program-as-and-object that we'll load into rundmc + // description / name is required to load successfully + var uistring = { + description: { + name: 'string-output', + alt: 'for clicking' + } + } + + // the State() object is what the system scrapes for ui variables / updates from the UI + // this includes things like Button('title', callback), which are unique state variables + // they can also be found in jsunit.js + uistring.state = State() + // alias ! + var state = uistring.state + + state.string = 'something' + + uistring.ui = UI() + var ui = uistring.ui + ui.addElement('onNumberButton', './ui/uiButton.js', onStringDesire) + ui.onNumberButton.onload = function() { + ui.onNumberButton.setText('number out ->') + } + + // inputs are required, and must be Input('type', callback) + uistring.inputs = { + thru: Input('any', onThruInput), // makes anything into string event + evt: Input('any', onStringDesire) + } + + // outputs: Output('type') + uistring.outputs = { + out: Output('string') + } + + // here's our input callback, specified in the input constructor + function onThruInput(input){ + if(typeof input == 'string'){ + state.string = input + } else { + state.string = input.toString() + } + onStringDesire() + } + + function onStringDesire(){ + // here's how we fire an output. + uistring.outputs.out.emit(state.string) + } + + // gotta give the program this thing we made + return uistring +} + +// this for node.js's require() function +module.exports = uiString \ No newline at end of file diff --git a/py/leastSquares.py b/py/leastSquares.py new file mode 100644 index 0000000000000000000000000000000000000000..965dd0ec66916c4238cfae3d845440538b21605c --- /dev/null +++ b/py/leastSquares.py @@ -0,0 +1,84 @@ +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 = int(cString) +d = int(dString) + +A = constructmatA(c, theta1s, theta2s) +b = constructmatb(d, theta1s) + +fullRes = np.linalg.lstsq(A,b) + +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) +''' \ No newline at end of file