Fix root coords

This patch fix the problem that there is a problem if root link has translation or rotation from origin.