Use apply joint state

use (apply-joint_state) to reduce redundant code