reinstated cubic for planner type 2

Author: Luca Toniolo

src/emc/motion/control.c

@@ -60,14 +60,6 @@ static unsigned long last_period = 0;
 /* servo cycle time */
 static double servo_period;
 
-/* Planner type 2: joint-space velocity feedforward via finite differences.
- * Correct for ALL kinematics (trivkins, tilting pivot, etc.) unlike the
- * previous direction-vector decomposition which assumed joints = XYZ axes. */
-static double p2_prev_pos[EMCMOT_MAX_JOINTS];
-static double p2_prev_vel[EMCMOT_MAX_JOINTS];
-static double p2_prev_acc[EMCMOT_MAX_JOINTS];
-static int p2_ff_init = 0;
-
 extern struct emcmot_status_t *emcmotStatus;
 
 // *pcmd_p[0] is shorthand for emcmotStatus->carte_pos_cmd.tran.x
@@ -1215,7 +1207,6 @@ static void get_pos_cmds(long period)
     /* run traj planner code depending on the state */
     switch ( emcmotStatus->motion_state) {
     case EMCMOT_MOTION_FREE:
-	p2_ff_init = 0;  /* Reset feedforward state when leaving coord mode */
 	/* in free mode, each joint is planned independently */
 	/* initial value for flag, if needed it will be cleared below */
 	SET_MOTION_INPOS_FLAG(1);
@@ -1365,61 +1356,44 @@ static void get_pos_cmds(long period)
 	}
 
 	if (emcmotStatus->planner_type == 2) {
-	    /* Planner type 2 (Ruckig): bypass cubic interpolator.
-	     * Joint velocity/acceleration/jerk feedforward computed from
-	     * finite differences of IK output — correct for ALL kinematics. */
+	    /* Planner type 2 (Ruckig): use cubic interpolation to smooth
+	     * directional discontinuities at bezier-to-line split boundaries.
+	     * The cubic [1,4,1]/6 waypoint filter spreads single-sample
+	     * direction jumps over 3 samples, eliminating jerk spikes. */
 	    EmcPose current_pos = emcmotStatus->carte_pos_cmd;
 
 	    result = kinematicsInverse(&current_pos, positions, &iflags, &fflags);
 	    if (result == 0) {
-		double inv_dt = 1.0 / servo_period;
-
 		for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) {
 		    if (!isfinite(positions[joint_num])) {
 			reportError(_("kinematicsInverse gave non-finite joint location on joint %d"),
 			    joint_num);
 			SET_MOTION_ERROR_FLAG(1);
 			emcmotInternal->enabling = 0;
-			p2_ff_init = 0;
 			break;
 		    }
 		    joint = &joints[joint_num];
 		    joint->coarse_pos = positions[joint_num];
-		    joint->pos_cmd = positions[joint_num];
-
-		    if (p2_ff_init) {
-			double vel = (positions[joint_num] - p2_prev_pos[joint_num]) * inv_dt;
-			double acc = (vel - p2_prev_vel[joint_num]) * inv_dt;
-			joint->vel_cmd = vel;
-			joint->acc_cmd = acc;
-			joint->jerk_cmd = (acc - p2_prev_acc[joint_num]) * inv_dt;
-			p2_prev_vel[joint_num] = vel;
-			p2_prev_acc[joint_num] = acc;
-		    } else {
-			joint->vel_cmd = 0.0;
-			joint->acc_cmd = 0.0;
-			joint->jerk_cmd = 0.0;
-			p2_prev_vel[joint_num] = 0.0;
-			p2_prev_acc[joint_num] = 0.0;
-		    }
-		    p2_prev_pos[joint_num] = positions[joint_num];
 		}
-		p2_ff_init = 1;
 
-		/* Still feed the cubic so it stays primed if we switch planner types */
+		/* Feed cubic interpolator */
 		while (cubicNeedNextPoint(&(joints[0].cubic))) {
 		    for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) {
 			cubicAddPoint(&(joints[joint_num].cubic), positions[joint_num]);
 		    }
 		}
+		/* Get smoothed position with analytical vel/acc/jerk */
+		for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) {
+		    joint = &joints[joint_num];
+		    joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0,
+			&(joint->vel_cmd), &(joint->acc_cmd), &(joint->jerk_cmd));
+		}
 	    } else {
 		reportError(_("kinematicsInverse failed"));
 		SET_MOTION_ERROR_FLAG(1);
 		emcmotInternal->enabling = 0;
-		p2_ff_init = 0;
 	    }
 	} else {
-	    p2_ff_init = 0;
 	    /* Planner type 0 and 1: use cubic interpolator as before */
 
 	    /* Fill cubic buffer if needed (may need multiple points per cycle) */
@@ -1495,7 +1469,6 @@ static void get_pos_cmds(long period)
 	break;
 
     case EMCMOT_MOTION_TELEOP:
-	p2_ff_init = 0;  /* Reset feedforward state when leaving coord mode */
         ext_offset_teleop_limit = axis_calc_motion(servo_period);
         if (!ext_offset_teleop_limit) {
             ext_offset_coord_limit = 0; //in case was set in prior coord motion