# 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!