try to remove jerkiness avoiding useless switch to pos ctrl mode, however it is still jerky
add joints to excite to configuration file