Work on replanning framework fix bug in human joint state