Add foot comp ports

Update Stabilizer codes.
- Add dataport log for foot compensation debug
- Update force z control for foot compensation debug