working fork for changes to the floating base implementation in drake