Fix joint state saving