Droid Project: Completed Simulation Body

Currently finished coupling 6 legs to a simulation body, with the leg inverse kinematics functional for all of the legs. The remaining elements to implement would be select-able individual joint controls for each leg and the body kinematics itself.

Then I completed the inverse kinematics that allows for the coupling between the body of the robot and its 6 corresponding legs (arranged with 60 degrees between each leg). I won’t bother explaining the mathematics going into these calculations in detail as they have been outlined in Oscar Liang’s blog post and the equations from TogleFritz’s Lair.

The resultant equations needed a bit of tweaking, especially with the orientation of the coordinate plane and the direction of certain rotations. Additionally, the equations from TogleFritz’s Lair are unnecessarily redundant when implemented in code. I added some optimizations and loop logic to help make the equations less cumbersome to use.

void Droid::ikCalculate() {
  const float increment = M_PI/3;

  for(int i = 0; i < 6; i++) {
    vec3 bodyOffset = vec3(cos(increment * i) * legRadius, 0, sin(increment * i) * legRadius);
    vec3 legPos = vec3(
        cos(increment * i) * (DEFAULT_COXA_LEN + DEFAULT_FEMUR_LEN), 
        -DEFAULT_TIBIA_LEN, 
        sin(increment * i) * (DEFAULT_COXA_LEN + DEFAULT_FEMUR_LEN));

    vec3 totalPos = bodyOffset + legPos + bodyPos;

    float distToLeg = sqrt(pow(totalPos[0], 2) + pow(totalPos[2], 2));
    float angleToLeg = atan2(totalPos[2], totalPos[0]);
 
    float roll = tan(bodyRot[2]) * totalPos[0]; // About the Z Axis
    float pitch = tan(bodyRot[0]) * totalPos[2]; // About the X Axis
    
    float bodyIkX = cos(angleToLeg + bodyRot[1]) * distToLeg - totalPos[0];
    float bodyIkY = roll + pitch;
    float bodyIkZ = sin(angleToLeg + bodyRot[1]) * distToLeg - totalPos[2];
  
    vec3 finalLegPos = legPos + vec3(bodyIkX, bodyIkY, bodyIkZ) + bodyPos;

    // Coordinate frame transform from body to leg (rotated)
    vec3 legCordFrame = vec3(
        cos(i * increment) * finalLegPos[0] + sin(i * increment) * finalLegPos[2],
        finalLegPos[1],
        -sin(i * increment) * finalLegPos[0] + cos(i * increment) * finalLegPos[2]);

    gl::drawVector(vec3(0,0,0), legCordFrame);

    mLeg[i].moveToCoord(&legCordFrame);
  }
}

With those changes and a couple of new ImGui control panels, everything is looking nice and functional!