FK for 2 legs
Edit Agent::run() for test FK
You can uncomment the "cout"s in Body::forwardKinematics for see pos of joints.
lj6 Pos is set to head of leg(Toe).