Hi Kurt,
I just send yah a PM.
Hope it’s ok to jump in…
A bit more info about the walking part in 2.0. Maybe it’s all clear but just to be sure 
When walking, the GaitEngine finds a new position in space where it wants to place the leg. The LegIK finds the needed servo angles to get the leg on the given point in space. With balance mode on, the body will keep in reference to all 6 legs at all time. Meaning that if one leg sticks out, the body will move a bit to that leg so it will be in the middle again. Also, if one leg goes up, the body goes up as well. This part is crucial to get TA to work.
For walking over small objects it will work to ONLY do FK for the legs and keep the balance mode off. This will not work for higher objects since the body position is reference is to the ground. If the object (or range of objects) will get higher then the body the bot will run in to problems. If the balance mode is on the body position is referred to all 6 legs. So if the legs are placed higher, the body will be places higher.
In the current code (2.0) the sub BodyIK handles the balance mode. But unlike the title of the sub says, the calculation is a global rotation matrix and not Inverse Kinematics. Actually, it is closer to FK. 
To get TA to work you need 2 extra parts of math. LegFK and BodyIK (Real IK this time )
How it should work:
- Drive the leg down to a given position (even can be in an angle)
- Once the TA switch is hit, stop all servos
- Query all positions from the SSC.
- Do FK for 3 outer legs which where on the ground during this step.
- Do body IK for from those 3 legs to the centerpoint of the body
- Do body IK for the 3 other legs (one or all of these legs where going down) This will give yah the XYZ position of the coxa.
- Those XYZ positions will be the starting point for to do FK from the body to the tars of the other 3 legs. This will give the XYZ position of where the leg stopped on the object.
In my tests I used this LegFK sub:
[code];--------------------------------------------------------------------
;[LEG FK] Calculates the position of the Tars corresponding to the given angles
LegFKLegNr var nib
LegFK [LegFKLegNr]
;Coxa angle
Gosub GetSinCos[CoxaAngle1(LegFKLegNr)-cCoxaAngle1(LegFKLegNr)]
FKSinC4 = Sin4
FKCosC4 = Cos4
;Femur angle
Gosub GetSinCos-FemurAngle1(LegFKLegNr)+900]
FKSinF4 = Sin4
FKCosF4 = Cos4
;Tibia angle
;Gosub GetSinCos(-FemurAngle1(LegFKLegNr)+900) + (TibiaAngle1(LegFKLegNr)-900)]
Gosub GetSinCos-FemurAngle1(LegFKLegNr) + TibiaAngle1(LegFKLegNr)]
FKSinT4 = Sin4
FKCosT4 = Cos4
;Distance between coxa and tars
FKSW2 = cCoxaLength + (FKSinF4cFemurLength + FKSinT4cTibiaLength)/c4DEC
;Position of the tars
FKFeetPosX = FKCosC4FKSW2/c4DEC
FKFeetPosY = (FKCosF4cFemurLength + FKCosT4cTibiaLength)/c4DEC
FKFeetPosZ = FKSinC4FKSW2/c4DEC
return
;--------------------------------------------------------------------[/code]
It is the inverse translation of the legIK sub. If I remember it correctly, this sub has some minor “round off?” errors. But I doubt it will matter with the given play in the servos.
This should give you the possibility to walk over small objects with balance mode off.
Now, to walk over larger objects you will need the balance mode. Step 3 gives 3 XYZ positions for the 3 coxas from the legs which where on the ground (known reference positions)
This will give a plain in 3D space. to get from those 3 coxa positions to the unknown 3 coxa positions we’ll need to do body IK. Since the body has a 3DOF rotation the best way to go is with a rotation matrix. To get a more precise function I’m rewriting the FK sub to a matrix version using the David Hartenberg method. This way I can include the body rotation in the leg FK so this will leave us with just one formula. Which will be easier to implement and more precise then the current method.
The David Hartenberg method is explained in the book “Applied Robotics” in chapter 5. Some additional information can be found on this wiki page
Other then the IK/FK we used in the code, this method uses global and local coordinate system. This makes it to flip the XYZ orientation. This makes it a bit hard to follow. Once this is done we can add the IK translation matrix for a spherical joint from Chapter 6.
Have you got this book? This would make it much easier to explain.
Hope this helps. Or maybe you’re thinking -> what is this guy talking about??? 
Xan
