follow_joint_traj test script working