Fix bugs from prev commit.

testWholeBodyPlanner passes now.