// so the first move can be correct if homing is not performed
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(machine_position, actuator_pos);
- for (size_t i = 0; i < n_motors; i++)
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
actuators[i]->change_last_milestone(actuator_pos[i]);
+ }
+
+ // initialize any extra axis to machine position
+ for (size_t i = A_AXIS; i < n_motors; i++) {
+ actuators[i]->change_last_milestone(machine_position[i]);
+ }
//this->clearToolOffset();