Add check for valid world pose cmd values

Add check for valid world pose cmd values #466