Added extractDofs and stackDofs helper functions
Added extractDofs and stackDofs helper functions

krishauser/Klampt,krishauser/Klampt,krishauser/Klampt,krishauser/Klampt,krishauser/Klampt,krishauser/Klampt