Adding leg controller part 2
target position for IK in swing legs calculated