# 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, 2) + pow(totalPos, 2));
float angleToLeg = atan2(totalPos, totalPos);

float roll = tan(bodyRot) * totalPos; // About the Z Axis
float pitch = tan(bodyRot) * totalPos; // About the X Axis

float bodyIkX = cos(angleToLeg + bodyRot) * distToLeg - totalPos;
float bodyIkY = roll + pitch;
float bodyIkZ = sin(angleToLeg + bodyRot) * distToLeg - totalPos;

vec3 finalLegPos = legPos + vec3(bodyIkX, bodyIkY, bodyIkZ) + bodyPos;

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

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!