Change postion_setpoint to position_sp