Hi,
I've spent some more time studying kinematics lately. I've worked more with Xan's code. If you study the subroutine BodyIK you can see that it takes care of Y-rotation and the pitch and roll rotation. I'm guessing Xan used the formulas I used in the PEP sheet for the roll and pitch calculation.
To be honest I've never been pleased with these simplified rotations (pitch and roll), because they are not real body rotation. They only uses FTAN to adjust the BodyIKPosY value.
If you compare these body rotations with the rotations Matt Denton demonstrates on his
you'll understand what I mean...
Anyway, I've rewrote the BodyIK subroutine:
Code:
BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]
;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
;PosY are equal to a "TotalY"
;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...
;First calculate sinus and cosinus for each rotation:
gosub GetSinCos [TOFLOAT(BodyRotX)]
sinG = sinA
cosG = cosA
gosub GetSinCos [TOFLOAT(BodyRotZ)]
sinB = sinA
cosB = cosA
gosub GetSinCos [TOFLOAT(BodyRotY+RotationY)]
;Calcualtion of rotation matrix:
BodyIKPosX = TotalX-TOINT(TOFLOAT(TotalX)*cosA*cosB - TOFLOAT(TotalZ)*cosB*sinA + TOFLOAT(PosY)*sinB)
BodyIKPosZ = TotalZ-TOINT(TOFLOAT(TotalX)*cosG*sinA + TOFLOAT(TotalX)*cosA*sinB*sinG +TOFLOAT(TotalZ)*cosA*cosG-TOFLOAT(TotalZ)*sinA*sinB*sinG-TOFLOAT(PosY)*cosB*sinG)
BodyIKPosY = PosY - TOINT(TOFLOAT(TotalX)*sinA*sinG - TOFLOAT(TotalX)*cosA*cosG*sinB + TOFLOAT(TotalZ)*cosA*sinG + TOFLOAT(TotalZ)*cosG*sinA*sinB + TOFLOAT(PosY)*cosB*cosG)
return
I've also modified other parts of the code.
BTW, I've just bought a
Futaba 7C 2,4 GHz RC remote control. This gives me much better control. I'm also going to modify the 7C. Posting more info about that later.
Here is a little video demonstration:
My wife are holding the cell phone camera
, she was actually impressed this time!
Sorry for the bad quality. When I find time for it I'll get my Sony video camera and make a real video/film.
