105 lines
3.4 KiB
Python
Executable File
105 lines
3.4 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import os, sys, json
|
|
from numpy import sin
|
|
from numpy import arcsin as asin
|
|
import numpy as np
|
|
|
|
def forward(D0,L1,L2,L3,L4,L5):
|
|
#define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
|
#define(r='L6',scl='L7')
|
|
#define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
|
#define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
|
L6=2000./500000
|
|
L7=5000/1.41421356237
|
|
L11=L7*sin(L1*1.74532925199e-05)
|
|
L12=L7*sin(L2*1.74532925199e-05)
|
|
L13=L7*sin(L3*1.74532925199e-05)
|
|
L14=L7*sin(L4*1.74532925199e-05)
|
|
L15=L7*sin(L5*1.74532925199e-05)
|
|
|
|
print("fwd_inp(%f) %f %f %f %f %f"%(D0,L1,L2,L3,L4,L5))
|
|
print("fwd_inp(%f) %f %f %f %f %f"%(D0,L11,L12,L13,L14,L15))
|
|
|
|
|
|
#//C6=D^-1*S
|
|
C6=+.5*L11 -.5*L12 +(L6+.25)*L13 +(L6-.75)*L14 +(.25-L6)*1.41421356237*L15
|
|
C7=+.5*L11 +.5*L12 +.25*L13 +.25*L14 +.25*1.41421356237*L15
|
|
C3= +1.*L13 +1.*L14 -1.*1.41421356237*L15
|
|
C4=-1.*L11 +1.*L12 +1.5*L13 -.5*L14 -.5*1.41421356237*L15
|
|
C5=-1.*L11 -1.*L12 +.5*L13 +.5*L14 +.5*1.41421356237*L15
|
|
|
|
print("fwd_res %f %f %f %f %f"%(C6,C7,C3,C4,C5))
|
|
|
|
#Dinv=[[+.5, -.5, +(L6+.25), +(L6-.75), +(.25-L6)*1.41421356237],
|
|
# [+.5, +.5, +.25, +.25, +.25*1.41421356237],
|
|
# [ 0, 0, +1., +1., -1.*1.41421356237],
|
|
# [-1., +1., +1.5, -.5, -.5*1.41421356237],
|
|
# [-1., -1., +.5, +.5, +.5*1.41421356237]]
|
|
#Dinv=np.matrix(Dinv)
|
|
#print(Dinv)
|
|
#print(Dinv**-1)
|
|
|
|
|
|
#D0=$000000f8; //C3=$8 C4=$10 C5=$20 C6=$40 C7=$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
|
|
#close
|
|
return (D0,C6,C7,C3,C4,C5)
|
|
|
|
def inverse(D0,C6,C7,C3,C4,C5):
|
|
#// define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
|
#// define(r='L6',scl='L7')
|
|
#// define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
|
#// define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
|
|
|
#//if(D0>0)
|
|
#// send 1"Velocity calculation NOT SUPPORTED\n"
|
|
|
|
print("inv_inp(%f) %f %f %f %f %f"%(D0,C6,C7,C3,C4,C5))
|
|
|
|
L6=2000./500000
|
|
|
|
L11=+.5*C6 +.5*C7 +(.25-.5*L6)*C3 -.25*C4 -.25*C5
|
|
L12=-.5*C6 +.5*C7 +(.5*L6-.25)*C3 +.25*C4 -.25*C5
|
|
L13=+.5*C6 +.5*C7 +(.25-.5*L6)*C3 +.25*C4 +.25*C5
|
|
L14=-.5*C6 +.5*C7 +(.25+.5*L6)*C3 -.25*C4 +.25*C5
|
|
L15= .5*1.41421356237*C7 -.25*1.41421356237*C3 +.25*1.41421356237*C5
|
|
|
|
L7=1.41421356237/5000
|
|
L1=asin(L11*L7)/1.74532925199e-05
|
|
L2=asin(L12*L7)/1.74532925199e-05
|
|
L3=asin(L13*L7)/1.74532925199e-05
|
|
L4=asin(L14*L7)/1.74532925199e-05
|
|
L5=asin(L15*L7)/1.74532925199e-05
|
|
|
|
print("inv_res %f %f %f %f %f"%(L1,L2,L3,L4,L5))
|
|
|
|
#D=[[+.5, +.5, +(.25-.5*L6), -.25, -.25],
|
|
# [-.5, +.5, +(.5*L6-.25), +.25, -.25],
|
|
# [+.5, +.5, +(.25-.5*L6), +.25, +.25],
|
|
# [-.5, +.5, +(.25+.5*L6), -.25, +.25],
|
|
# [ 0, .5*1.41421356237, -.25*1.41421356237, 0, +.25*1.41421356237]]
|
|
|
|
#D=np.matrix(D)
|
|
#print(D)
|
|
#print(D**-1)
|
|
|
|
|
|
return (D0,L1,L2,L3,L4,L5)
|
|
|
|
#close
|
|
|
|
|
|
|
|
f=forward(0,0,0,0,0,0)
|
|
print('')
|
|
i=inverse(*f)
|
|
print('\n')
|
|
f=forward(0,10000,20000,30000,40000,50000)
|
|
print('')
|
|
i=inverse(*f)
|
|
|
|
#SOMETHING IS WRONG IN THE COORD TRF !!!
|
|
|
|
|
|
|
|
#inverse(0,0,0,0,0,0)
|