Sensor to robot position 3d

- better use sensor_to_robot_position_3d in imu and velocity sensors
- more precise doc for sensor_to_robot_position_3d which fixes #394