Added constraint support to DynTree and iCubTree