bhand: Properly constructing KDL subtree
bhand: Properly computing COM and Properly constructing KDL subtree (for real)