Initialize current_position correctly during startup

Just after setting up the w2m matrix, call "clamp_to_software_endstops"
on the current_position (initially [0,0,0]) to move it to the effective
minimal position, which is usually [0,0,non-zero] due to MIN_Z and the
negative probe offset.

This is required to calculate correctly the first relative move:
planning X+10 would unexpectedly calculate a Z shift otherwise.

Firmware/Marlin_main.cpp

@@ -1310,10 +1310,17 @@ void setup()
 	setup_photpin();
 
 	servo_init();
+
 	// Reset the machine correction matrix.
 	// It does not make sense to load the correction matrix until the machine is homed.
 	world2machine_reset();
-    
+
+    // Initialize current_position accounting for software endstops to
+    // avoid unexpected initial shifts on the first move
+    clamp_to_software_endstops(current_position);
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS],
+                      current_position[Z_AXIS], current_position[E_AXIS]);
+
 #ifdef FILAMENT_SENSOR
 	fsensor_init();
 #endif //FILAMENT_SENSOR