Omni-segway

This is one of my part-script models. I use scripts to effectively attach a ball to the end of a chip. This doesn't use any libraries.

I think I just need to modify it to use the velocity of the center of gravity, rather than one chip.

wheel.rcs

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 vmult(k,v)
  return {k*v[1],k*v[2],k*v[3]}
end

function vadd(v1,v2)
  return {v1[1]+v2[1],v1[2]+v2[2],v1[3]+v2[3]}
end

function OnFrame()
  local r = rot({0,0,-1,0},{_QX(WHEEL),_QY(WHEEL),_QZ(WHEEL),_QW(WHEEL)})
  s0 = s
  s = {_X(WHEEL)+r[1]-_OX(0),_Y(WHEEL)+r[2]-_OY(0),_Z(WHEEL)+r[3]-_OZ(0)}
  local v = vadd(s,vmult(-1,s0))
  local f = vadd(vmult(100000,s),vmult(100000,v))
  if(math.abs(f[2]) < 1000000) then
    _FORCEOBJ(0,f[1],f[2],f[3])
    _FORCE(WHEEL,-f[1],-f[2],-f[3])
  end

  _TORQUEOBJ(0,TX,TY,TZ)
  _TORQUE(WHEEL,-TX,-TY,-TZ)
end

function OnInit()
  _WARP(0,_X(),_Y()+5,_Z())
  local r = rot({0,0,-1,0},{_QX(WHEEL),_QY(WHEEL),_QZ(WHEEL),_QW(WHEEL)})
  s = {_X(WHEEL)+r[1]-_OX(0),_Y(WHEEL)+r[2]-_OY(0),_Z(WHEEL)+r[3]-_OZ(0)}
  s0 = {}
  _ADDBALL(0.5,_X(WHEEL)+r[1],_Y(WHEEL)+r[2],_Z(WHEEL)+r[3],0.05)
--(size,x,y,z,density)
--  _SETOBJCOLOR(0,0,0,0)
end

function OnReset()

end

function OnMode()

end


omnisegway.txt

Code:


Val
{
   X(default=0)
   Y(default=0)
   Z(default=0)
   TX(default=0,min=-10000,max=10000,step=10000)
   TY(default=0,min=-10000,max=10000,step=10000)
   TZ(default=0,min=-10000,max=10000,step=10000)
        START(default=1)
        V(default=0,min=-6,max=6,step=0.5)
   EY(default=0,min=-7,max=7)
}
Key
{
   0:V(step=-0.5)
   1:V(step=0.5)
   2:EY(step=-0.1)
   3:EY(step=0.1)
}
Body {
  Core(){
    N:Chip(){}
    N:Chip(angle=90,name=WHEEL){}
  }
}
Lua {
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 main()
  if EY < -math.pi then
    EY = EY + 2*math.pi
  else if EY > math.pi then
    EY = EY - 2*math.pi
  end end
  if START == 1 then
    ang = {_QX(WHEEL),_QY(WHEEL),_QZ(WHEEL),_QW(WHEEL)}
    START = 0
  end
  ang0 = ang
  ang = {_QX(WHEEL),_QY(WHEEL),_QZ(WHEEL),_QW(WHEEL)}
  dir = rot({0,0,1,0},{_QX(WHEEL),_QY(WHEEL),_QZ(WHEEL),_QW(WHEEL)})
  local dang = qmult(ang,qinv(ang0))
  VX = V*math.sin(EY)
  VZ = V*math.cos(EY)
  TX = 10000*(dir[3]+0.8*math.atan((_VZ()-VZ)/5)) + 50000*dang[1]
  TY = 10000*dang[2]+1000*math.sin(_EY()-EY)
  TZ = -10000*(dir[1]+0.8*math.atan((_VX()-VX)/5)) + 50000*dang[3]
out(4,_VZ())
--TX = -10000*dang[1]
--TY = -10000*ddir[2]
--TZ = -10000*ddir[1]
out(0,dang[1])
out(1,dang[2])
out(2,dang[3])
end
}