Jetcalc

View previous topic View next topic Go down

Jetcalc

Post by DanielLC on Thu Oct 28, 2010 6:52 pm

Jetcalc is designed to take a model with more than six jets, and use it to give a certain acceleration and torque. It will minimize the sum of the squares of the jet powers.

I'd explain how to use it, but you're probably better off reverse engineering Hand.

I think I'm too new here to be allowed to upload or something, so make a folder labeled DanielLC in your Data folder, and put these files in it:

jetcalc.lua
Code:
require("DanielLC/matrix.lua")
require("DanielLC/quaternion.lua")

function center()
  local s={0,0,0}
  for i=0, _CHIPS() do
    s[1]=s[1]+_M(i)*_X(i)
    s[2]=s[2]+_M(i)*_Y(i)
    s[3]=s[3]+_M(i)*_Z(i)
  end
  s[1]=s[1]/_WEIGHT()
  s[2]=s[2]/_WEIGHT()
  s[3]=s[3]/_WEIGHT()
  return(s)
end
function leastsquares(matrix, rows, cols, vector)         --vector=matrix*v, returns v that minimizes vector.
  local m=trans(matrix,rows,cols)
  m=mmult(m,cols,rows,matrix,rows,cols)
  m=minv(m,cols)
  m=mmult(m,cols,cols,trans(matrix,rows,cols),cols,rows)
  local v=mvmult(m,cols,rows,vector)
  return v
end
function radius(jet,cog)
  return({_X(jet)-cog[1],_Y(jet)-cog[2],_Z(jet)-cog[3]})
end
function pmi(chip)      -- principal moment of inertia
   local typ = _TYPE(chip)
   if typ == 0 or typ == 1 or typ == 2 or typ == 6 then
      return {0.7770,1.5120,0.7770}
   elseif typ == 33 or typ == 34 or typ == 35 then
      return {0.3806,0.7560,0.3806}
   elseif typ == 3 then
      return {1.5109,0.4771,1.5109}
   elseif typ == 4 or typ == 5 then
      local op = _OPTION(chip)
      if op == 0 then
         return {0.8577,1.6450,0.8577}
      elseif op == 1 then
         return {1.8669,3.7216,1.8669}
      elseif op == 2 then
         return {3.3100,6.6162,3.3100}
      end
   elseif typ == 8 then
      local op = _OPTION(chip)
      if op == 0 then op = 1 end
      return {3.1080*op,6.0480*op,3.1080*op}
   elseif typ == 7 then
      return {0.8577,1.6540,0.8577}
   elseif typ == 10 then
      return {1.5540,3.0240,1.5540}
   end
end
function mit(chip,cog)      --Moment of inerta tensor
  local D = diag(pmi(chip),3)
  local Rot = mrot(_EX(chip),_EY(chip),_EZ(chip))
  local T = mmult(Rot,3,3,D,3,3)
  D = diag({(_EX(chip)-cog[1])^2,(_EY(chip)-cog[2])^2,(_EZ(chip)-cog[3])^2},3)
  return madd(T,D,3,3)
end
function invmit(cog)      --This is for the whole model, rather than one chip.
  local M = makeM(3,3)
  for i=0, _CHIPS() do
    madd(M,mit(i,cog),3,3)
  end
  return minv(M,3)
end
function center()
  local s={0,0,0}
  for i=0, _CHIPS() do
    s[1]=s[1]+_M(i)*_X(i)
    s[2]=s[2]+_M(i)*_Y(i)
    s[3]=s[3]+_M(i)*_Z(i)
  end
  s[1]=s[1]/_WEIGHT()
  s[2]=s[2]/_WEIGHT()
  s[3]=s[3]/_WEIGHT()
  return(s)
end
function angacc(alpha,cog)   --Converts angular velocity to angular acceleration
  return mvmult(invmit(cog),3,3,alpha)
end
function radius(jet,cog)
  return({_X(jet)-cog[1],_Y(jet)-cog[2],_Z(jet)-cog[3]})
end
function jetcalc(Ji,thrust,jets)   --For when you have at least six jets
  local HIGHT=6
  local WIDTH=jets+1
  local cog=center()
  local M=makeM(HIGHT,WIDTH)
  local i
  local j
  for j=1,jets do
    dir=rot({0,1,0,0},{_QX(Ji[j]),_QY(Ji[j]),_QZ(Ji[j]),_QW(Ji[j])})   --dir is the way jet j is facing.
    M[1][j]=dir[1]                     --the x, y, and z components of the engines must add to (0,y,0)
    M[2][j]=dir[2]
    M[3][j]=dir[3]
    torque=cross(dir,radius(Ji[j],cog))
    M[4][j]=torque[1]
    M[5][j]=torque[2]
    M[6][j]=torque[3]
  end
  for i=1,HIGHT do
    M[i][WIDTH]=thrust[i]*_WEIGHT()*6
  end
  local t=angacc({thrust[4],thrust[5],thrust[6]},cog)
  for i=1,3 do
    M[i+3][WIDTH]=t[i]
  end
  rref(M,HIGHT,WIDTH)
  local Sg,sp,rows,cols=solve(M,HIGHT,WIDTH)            --Sg = general solution (matrix), sp = particular solution (vector)
                           --for all v, J = Sg * v + sp
  local v2=leastsquares(Sg,rows,cols,vminus(sp,rows))         --returns v2 such that |Sg * v2 - (-sp)| is minimized
  local del=mvmult(Sg,rows,cols,v2)
  local Jo=vadd(del,sp,rows)
  return Jo
end
function jetcalc2(Ji,thrust)      --For when you have exactly six jets
  local cog=center()
  local HIGHT=6
  local WIDTH=7
  local M=makeM(HIGHT,WIDTH)
  local i
  local j
  for j=1,HIGHT do
    dir=rot({0,1,0,0},{_QX(Ji[j]),_QY(Ji[j]),_QZ(Ji[j]),_QW(Ji[j])})
    M[1][j]=dir[1]
    M[2][j]=dir[2]
    M[3][j]=dir[3]
    torque=cross(dir,radius(Ji[j],cog))
    M[4][j]=torque[1]
    M[5][j]=torque[2]
    M[6][j]=torque[3]
  end
  for i=1,3 do
    M[i][WIDTH]=thrust[i]*_WEIGHT()*6
  end
  local t=angacc({thrust[4],thrust[5],thrust[6]},cog)
  for i=1,3 do
    M[i+3][WIDTH]=t[i]
  end
  local Jo=makeV(HIGHT)
  rref(M,HIGHT,WIDTH)
  for i=1,HIGHT do
    Jo[i]=M[i][WIDTH]
  end
  return Jo
end

matrix.lua
Code:
M=function(i,m,MATRIX,WIDTH)            --Multiply row i by m
  local j
  for j=1,WIDTH do
    MATRIX[i][j]=m*MATRIX[i][j]
  end
end
A=function(i,j,m,MATRIX,WIDTH)            --Add m*row i to row j  YOU ARE HERE
  local c
  for c=1,WIDTH do
    MATRIX[j][c]=m*MATRIX[i][c]+MATRIX[j][c]
  end
end
function rref(MATRIX,HIGHT,WIDTH)         --Reduce
  local lead=1
  local i
  local j
  for j=1,WIDTH-1 do
    if MATRIX[lead][j]~=0 then
      M(lead,1/MATRIX[lead][j],MATRIX,WIDTH)
      for i=1,HIGHT do
        if i~=lead then
          A(lead,i,-MATRIX[i][j],MATRIX,WIDTH)
        end
      end
      if lead>=HIGHT then
        break
      end
      lead=lead+1
    end
  end
end
function makeV(size)
  local i
  local vector={}
  for i=1,size do
    table.insert(vector,0)
  end
  return vector
end
function makeM(rows,cols)
  local matrix={}
  local i
  local j
  for i=1,rows do
    table.insert(matrix,{})
    for j=1,cols do
      table.insert(matrix[i],0)
    end
  end
  return matrix
end
function solve(m0,rows,cols)               --takes an aumented matrix in RREF, and returns the general and particular solutions
  local m1=makeM(cols-1,cols-rows-1)
  local v=makeV(cols-1)
  local i
  local j
  for i=1,rows do
    for j=1,cols-rows-1 do
      m1[i][j]=-m0[i][j+rows]
    end
    v[i]=m0[i][cols]
  end
  for j=1,cols-rows-1 do
    m1[rows+j][j]=1
  end
  return m1,v,cols-1,cols-rows-1
end
function trans(matrix,rows,cols)
  local m=makeM(cols,rows)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      m[j][i]=matrix[i][j]
    end
  end
  return m
end
function mmult(m0,rows,diag,m1,asdf,cols)         --rows is the number of rows on the left matrix, cols is the number of columns on the right, and diag=asdf is the number of cols on left and rows on right
  local m=makeM(rows,cols)
  local i
  local j
  local d
  for i=1,rows do
    for j=1,cols do
      for d=1,diag do
        m[i][j]=m[i][j]+m0[i][d]*m1[d][j]
      end
    end
  end
  return m
end
function minv(matrix,size)               --this requires a square matrix, so only one dimension is necessary
  local m0=makeM(size,2*size)
  local m1=makeM(size,size)
  local i
  local j
  for i=1,size do
    for j=1,size do
      m0[i][j]=matrix[i][j]
    end
  end
  for i=1,size do
    m0[i][size+i]=1
  end
  rref(m0,size,2*size)
  for i=1,size do
    for j=1,size do
      m1[i][j]=m0[i][size+j]
    end
  end
  return m1
end
function mvmult(matrix,rows,cols,vector)
  local v=makeV(rows)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      v[i]=v[i]+matrix[i][j]*vector[j]
    end
  end
  return v
end
function vadd(v1, v2, size)
  local v=makeV(size)
  local i
  local j
  for i=1,size do
    v[i]=v1[i]+v2[i]
  end
  return v
end
function mout(matrix, rows, cols, pos)
  local i
  local j
  for i=1,rows do
  local txt=""
  for j=1,cols do
  txt=txt..(matrix[i][j])..",  "
  end
  out(i+pos,txt)
  end
end
function vminus(vector,size)
  local v = {}
  local i
  for i=1,size do
    table.insert(v,-vector[i])
  end
  return v
end
function cross(v1,v2)
  local v={0,0,0}
  v[1]=v1[2]*v2[3]-v1[3]*v2[2]
  v[2]=v1[3]*v2[1]-v1[1]*v2[3]
  v[3]=v1[1]*v2[2]-v1[2]*v2[1]
  return(v)
end
function madd(M1,M2,rows,cols)
  local M = makeM(rows,cols)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      M[i][j]=M1[i][j]+M2[i][j]
    end
  end
  return M
end
function mrot(x,y,z)
  local Z = {{math.cos(z),-math.sin(z), 0},
            {math.sin(z), math.cos(z), 0},
            {          0,          0, 1}}

  local Y = {{ math.cos(y), 0,math.sin(y)},
            {          0, 1,          0},
            {-math.sin(y), 0,math.cos(y)}}

  local X = {{1,          0,          0},
            {0, math.cos(x),-math.sin(x)},
            {0, math.sin(x), math.cos(x)}}

  return mmult(Z,3,3,mmult(Y,3,3,X,3,3),3,3)
end
function diag(vector,size)
  local D = makeM(size,size)
  local i
  for i=1,size do
    D[i][i] = vector[i]
  end
  return D
end

quaternion.lua
Code:
function qmult(q1,q2)
  local q3={0,0,0,0}
  q3[1]=q1[2]*q2[3]-q1[3]*q2[2]+q1[4]*q2[1]+q1[1]*q2[4]
  q3[2]=q1[3]*q2[1]-q1[1]*q2[3]+q1[4]*q2[2]+q1[2]*q2[4]
  q3[3]=q1[1]*q2[2]-q1[2]*q2[1]+q1[4]*q2[3]+q1[3]*q2[4]
  q3[4]=q1[4]*q2[4]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]
  return(q3)
end
function qinv(q)
  return({-q[1],-q[2],-q[3],q[4]})
end
function rot(v,q)
  return qmult(qmult(q,v),qinv(q))
end
function q2vector(q)
  local v = makeV(3)
  local alpha12 = math.acos(q[4])   --1/2 alpha
  local scale = math.sin(alpha12)*mod(2*alpha12)
  v[1] = q[1]*scale
  v[2] = q[2]*scale
  v[3] = q[3]*scale
  return v
end


Last edited by DanielLC on Fri Nov 26, 2010 8:17 pm; edited 1 time in total (Reason for editing : Update)

DanielLC
Tank
Tank

Posts : 78
Join date : 2010-10-23

View user profile

Back to top Go down

Re: Jetcalc

Post by bwansy on Fri Oct 29, 2010 12:15 am

Another engineering major on the forums. Very Happy

_________________
A.K.A. Bernard

bwansy
Admin

Posts : 170
Join date : 2010-07-15

View user profile http://rigidchips.forum-motion.com

Back to top Go down

Re: Jetcalc

Post by JHaskly on Tue Nov 09, 2010 12:38 am

I just noticed that you need a WIDTH arguement (or rows, columns, diagonal, etc.) to most of your functions in matrix.lua, is there any reason why you didn't use table.len instead?

Also, incase you don't know about metatables, it would be possible to adjust your code slightly so that you can use regular operators (like + * - ^ etc., and equality operators) with the matrices, rather than functions like vadd(), vminus(), mmult(), etc.

_________________
Previously (and currently) known as Juz.

Please do not write "LUA", beause it's "Lua". It's Moon in Portuguese, not a Limited User Account, Last Universal Ancestor, or the Lukla Airport in Nepal.

JHaskly
Admin

Posts : 235
Join date : 2010-07-16
Age : 21
Location : Brisbane

View user profile

Back to top Go down

Re: Jetcalc

Post by DanielLC on Wed Nov 10, 2010 2:56 pm

I didn't know about either. I'll eventually get around to updating those.

DanielLC
Tank
Tank

Posts : 78
Join date : 2010-10-23

View user profile

Back to top Go down

Re: Jetcalc

Post by JHaskly on Wed Nov 10, 2010 11:32 pm

Smile

EDIT: My mistake, it's actually table.getn()

_________________
Previously (and currently) known as Juz.

Please do not write "LUA", beause it's "Lua". It's Moon in Portuguese, not a Limited User Account, Last Universal Ancestor, or the Lukla Airport in Nepal.

JHaskly
Admin

Posts : 235
Join date : 2010-07-16
Age : 21
Location : Brisbane

View user profile

Back to top Go down

Re: Jetcalc

Post by Sponsored content


Sponsored content


Back to top Go down

View previous topic View next topic Back to top


 
Permissions in this forum:
You cannot reply to topics in this forum