c1a02b39f720a12f4d3b37c2c95bd2af.mp4
compiled from https://github.com/JoergS5/RepRapFirmware_robot, based on RRF 3.5.1
thanks for @JoergS5's efforts

@JoergS5 I was wondering if we could collaborate to maintain the five-axis kinematics code for RepRapFirmware and submit pull requests on GitHub. I am a master’s student in the School of Mechanical Engineering at Zhejiang University, currently researching five-axis printing. I believe contributing such code to RRF would be both interesting and meaningful.

@Mafco77 Same problem. It seems that the firmware does not seem to support G1 Hxxx, maybe... I'm not quite sure...
I have a trick which is to use M669 K1 first, then home all axes and use M669 K13 B "CoreXY5AC".
Not perfect but workable.

b860ca50-40b5-49c6-a01a-6eee3e6353da-373f17267ac28bce0491e00515ed19fd.mp4

Fixed a bug where, after giving a G1 command that includes only BC values without XYZ, that is, when the nozzle's XYZ coordinates in the workpiece coordinate system remain unchanged and only the BC axis angles are changed, the nozzle moves in a straight line in the machine coordinate system. It should actually move in a curve to keep the nozzle's XYZ coordinates in the workpiece coordinate system constant during the movement.

@droftarts Segmenting the moves does help. There are some small errors in @JoergS5's firmware, and I plan to discuss them with him in the coming days. However, the main issue lies in the GCodes.cpp file in the RepRapFirmware source code. I don't mean that it's a bug in RepRapFirmware itself, but rather that certain parts of the source code need to be modified to adapt to this special kinematics. The code in the DoStraightMove function in GCodes.cpp needs to be changed.

// Apply segmentation if necessary
		// As soon as we set segmentsLeft nonzero, the Move process will assume that the move is ready to take, so this must be the last thing we do.
#if SUPPORT_LASER
		if (machineType == MachineType::laser && isCoordinated && ms.laserPixelData.numPixels > 1)
		{
			ms.totalSegments = ms.laserPixelData.numPixels;			// we must use one segment per pixel
		}
		else
#endif
		{
			const Kinematics& kin = reprap.GetMove().GetKinematics();
			const SegmentationType st = kin.GetSegmentationType();
			// To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating.
			if (st.useSegmentation && simulationMode != SimulationMode::normal && (ms.hasPositiveExtrusion || ms.isCoordinated || st.useG0Segmentation))
			{
				// This kinematics approximates linear motion by means of segmentation
				float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
				if (st.useZSegmentation)
				{
					moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
				}
				const float moveLength = fastSqrtf(moveLengthSquared);
				const float moveTime = moveLength/(ms.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
				ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
			}
			else
			{
				ms.totalSegments = 1;
			}
		...
		}

Obviously, when the XYZ coordinates don't change, moveLength=0, and at the same time, totalSegments=1, which causes the segments to have no effect at all. As a result, the nozzle can only move along a straight line.
Although my current code runs fine, the motion accuracy doesn't seem to be very precise, and my code is also too dirty, so it requires further modifications.

I'm wondering if the SBC mode would offload part of the computation to the SBC, thereby improving computational efficiency.

@JoergS5 I was wondering if we could collaborate to maintain the five-axis kinematics code for RepRapFirmware and submit pull requests on GitHub. I am a master’s student in the School of Mechanical Engineering at Zhejiang University, currently researching five-axis printing. I believe contributing such code to RRF would be both interesting and meaningful.

@JoergS5 Your code indeed implements the correct five-axis kinematic calculations. However, in the DoStraightMove function of the RRF source code (GCodes.cpp), the segmentation is calculated after determining the moveLength.

// This kinematics approximates linear motion by means of segmentation
float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
if (st.useZSegmentation)
{
	moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
}
const float moveLength = fastSqrtf(moveLengthSquared);
const float moveTime = moveLength/(ms.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));

With the introduction of the BC axis, the movement becomes nonlinear. When the XYZ coordinates remain unchanged, the original DoStraightMove function would interpret the moveLength as 0, resulting in a calculated segmentation of 1. Therefore, we need to calculate the true moveLength to ensure that the correct segmentation is derived when the XYZ coordinates do not change.

@JoergS5 The video shows the result after I corrected the code bug. Before fixing the bug, the nozzle didn’t follow the needle movement on the table but instead traveled in a straight line from the starting point to the endpoint.

@droftarts Ok, I got it, sry

GCodes.cpp

if (st.useSegmentation && simulationMode != SimulationMode::normal && (ms.hasPositiveExtrusion || ms.isCoordinated || st.useG0Segmentation))
{
    if (!(strcmp(kin.GetName(), "robot")) && st.useZSegmentation)
    {
        const MessageType mt = (MessageType)(gb.GetResponseMessageType() | PushFlag);
        // Initialize the starting and ending XYZ coordinates
        float startPos[3] = {initialUserPosition[X_AXIS], initialUserPosition[Y_AXIS], initialUserPosition[Z_AXIS]}; // Example start point
        float endPos[3] = {ms.currentUserPosition[X_AXIS], ms.currentUserPosition[Y_AXIS], ms.currentUserPosition[Z_AXIS]};   // Example end point

        // Initialize the starting and ending BC rotation angles
        float startBeta = initialUserPosition[3];  // Example start Beta angle (around the Y-axis)
        float endBeta = ms.currentUserPosition[3];    // Example end Beta angle
        float startGamma = initialUserPosition[4]; // Example start Gamma angle (around the Z-axis)
        float endGamma = ms.currentUserPosition[4];   // Example end Gamma angle

        // Set the number of steps, the more steps, the more accurate the calculation
        int steps = 1000; // Example number of steps, 1000 steps

        // Call the function to calculate the total path length
        float moveLength = calculateTotalPathLength(startPos, endPos, startBeta, endBeta, startGamma, endGamma, steps);
        const float moveTime = moveLength/(ms.feedRate * StepClockRate);    // this is a best-case time, often the move will take longer
        ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
        reprap.GetPlatform().MessageF(mt,"1; moveLength:%.4f; totalSegments:%d\n", moveLength, ms.totalSegments);
    }
    else
    {
        // This kinematics approximates linear motion by means of segmentation
        float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
        if (st.useZSegmentation)
        {
            moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
        }
        const float moveLength = fastSqrtf(moveLengthSquared);
        const float moveTime = moveLength/(ms.feedRate * StepClockRate);    // this is a best-case time, often the move will take longer
        ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
        const MessageType mt = (MessageType)(gb.GetResponseMessageType() | PushFlag);
        reprap.GetPlatform().MessageF(mt,"2; moveLength:%.4f; totalSegments:%d\n", moveLength, ms.totalSegments);
    }
}
else
{
    ms.totalSegments = 1;
}

GCodes.h

const float radiansToDegrees = 57.295784f;

// Rotation matrix R_B (rotation around the Y-axis)
void rotationMatrixB(float beta, float matrix[3][3]) {
    float rad_beta = beta/radiansToDegrees;
    matrix[0][0] = cos(rad_beta); matrix[0][1] = 0; matrix[0][2] = sin(rad_beta);
    matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
    matrix[2][0] = -sin(rad_beta); matrix[2][1] = 0; matrix[2][2] = cos(rad_beta);
}

// Rotation matrix R_C (rotation around the Z-axis)
void rotationMatrixC(float gamma, float matrix[3][3]) {
    float rad_gamma = gamma/radiansToDegrees;
    matrix[0][0] = cos(rad_gamma); matrix[0][1] = -sin(rad_gamma); matrix[0][2] = 0;
    matrix[1][0] = sin(rad_gamma); matrix[1][1] = cos(rad_gamma); matrix[1][2] = 0;
    matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
}

// Matrix multiplication
void multiplyMatrix(float A[3][3], float B[3][3], float result[3][3]) {
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            result[i][j] = 0;
            for (int k = 0; k < 3; ++k) {
                result[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

// Matrix-vector multiplication
void multiplyMatrixVector(float matrix[3][3], const float vec[3], float result[3]) {
    for (int i = 0; i < 3; ++i) {
        result[i] = 0;
        for (int j = 0; j < 3; ++j) {
            result[i] += matrix[i][j] * vec[j];
        }
    }
}

// Calculate the Euclidean distance between two points
float distance(const float point1[3], const float point2[3]) {
    return fastSqrtf(fsquare(point2[0] - point1[0]) + fsquare(point2[1] - point1[1]) + fsquare(point2[2] - point1[2]));
}

// Interpolation calculation (linear interpolation)
float interpolate(float start, float end, float t) {
    return start + t * (end - start);
}

// Calculate the total path length
float calculateTotalPathLength(
    const float startPos[3], const float endPos[3],
    float startBeta, float endBeta, float startGamma, float endGamma, int steps)
{
    float totalLength = 0.0;

    float prevPosition[3] = {startPos[0], startPos[1], startPos[2]};
    float prevBeta = startBeta;
    float prevGamma = startGamma;

    // Rotation matrix cache
    float prerotationMatrixB_i[3][3], prerotationMatrixC_i[3][3], prerotationMatrixTotal_i[3][3];
    float rotationMatrixB_i[3][3], rotationMatrixC_i[3][3], rotationMatrixTotal_i[3][3];
    float prevMachinePosition[3], currentMachinePosition[3];

    for (int i = 1; i <= steps; ++i) {
        float t = (float)i / steps;

        // Interpolate the current position
        float currentPosition[3] = {
            interpolate(startPos[0], endPos[0], t),
            interpolate(startPos[1], endPos[1], t),
            interpolate(startPos[2], endPos[2], t)
        };

        float currentBeta = interpolate(startBeta, endBeta, t);
        float currentGamma = interpolate(startGamma, endGamma, t);

        rotationMatrixB(prevBeta, prerotationMatrixB_i);
        rotationMatrixC(prevGamma, prerotationMatrixC_i);
        multiplyMatrix(prerotationMatrixC_i, prerotationMatrixB_i, prerotationMatrixTotal_i);
        // Calculate the current rotation matrix
        rotationMatrixB(currentBeta, rotationMatrixB_i);
        rotationMatrixC(currentGamma, rotationMatrixC_i);
        multiplyMatrix(rotationMatrixC_i, rotationMatrixB_i, rotationMatrixTotal_i);

        // Calculate the current step's position
        multiplyMatrixVector(prerotationMatrixTotal_i, prevPosition, prevMachinePosition);
        multiplyMatrixVector(rotationMatrixTotal_i, currentPosition, currentMachinePosition);

        // Calculate the distance between the two points
        totalLength += distance(prevMachinePosition, currentMachinePosition);

        // Update the previous point
        prevPosition[0] = currentPosition[0];
        prevPosition[1] = currentPosition[1];
        prevPosition[2] = currentPosition[2];
        prevBeta = currentBeta;
        prevGamma = currentGamma;
    }

    return totalLength;
}

I only considered XYZBC printers.

Currently, when I calculate the moveLength, I use linear interpolation, which means maintaining a constant velocity in the machine coordinate system. However, my actual requirement is for the nozzle to move at a constant speed in the workpiece coordinate system. My previous method leads to discrepancies between the calculated moveLength and the actual value, which is one of the issues. The more significant problem now is that I want the nozzle to move at a given feedrate in the workpiece coordinate system, not at a constant speed in the machine coordinate system. I want to figure out which parts of the code I should modify to address this issue.@JoergS5 @dc42

@droftarts Segmenting the moves does help. There are some small errors in @JoergS5's firmware, and I plan to discuss them with him in the coming days. However, the main issue lies in the GCodes.cpp file in the RepRapFirmware source code. I don't mean that it's a bug in RepRapFirmware itself, but rather that certain parts of the source code need to be modified to adapt to this special kinematics. The code in the DoStraightMove function in GCodes.cpp needs to be changed.

// Apply segmentation if necessary
		// As soon as we set segmentsLeft nonzero, the Move process will assume that the move is ready to take, so this must be the last thing we do.
#if SUPPORT_LASER
		if (machineType == MachineType::laser && isCoordinated && ms.laserPixelData.numPixels > 1)
		{
			ms.totalSegments = ms.laserPixelData.numPixels;			// we must use one segment per pixel
		}
		else
#endif
		{
			const Kinematics& kin = reprap.GetMove().GetKinematics();
			const SegmentationType st = kin.GetSegmentationType();
			// To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating.
			if (st.useSegmentation && simulationMode != SimulationMode::normal && (ms.hasPositiveExtrusion || ms.isCoordinated || st.useG0Segmentation))
			{
				// This kinematics approximates linear motion by means of segmentation
				float moveLengthSquared = fsquare(ms.currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(ms.currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
				if (st.useZSegmentation)
				{
					moveLengthSquared += fsquare(ms.currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
				}
				const float moveLength = fastSqrtf(moveLengthSquared);
				const float moveTime = moveLength/(ms.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
				ms.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
			}
			else
			{
				ms.totalSegments = 1;
			}
		...
		}

Obviously, when the XYZ coordinates don't change, moveLength=0, and at the same time, totalSegments=1, which causes the segments to have no effect at all. As a result, the nozzle can only move along a straight line.
Although my current code runs fine, the motion accuracy doesn't seem to be very precise, and my code is also too dirty, so it requires further modifications.

b860ca50-40b5-49c6-a01a-6eee3e6353da-373f17267ac28bce0491e00515ed19fd.mp4

Fixed a bug where, after giving a G1 command that includes only BC values without XYZ, that is, when the nozzle's XYZ coordinates in the workpiece coordinate system remain unchanged and only the BC axis angles are changed, the nozzle moves in a straight line in the machine coordinate system. It should actually move in a curve to keep the nozzle's XYZ coordinates in the workpiece coordinate system constant during the movement.

@Mafco77 Same problem. It seems that the firmware does not seem to support G1 Hxxx, maybe... I'm not quite sure...
I have a trick which is to use M669 K1 first, then home all axes and use M669 K13 B "CoreXY5AC".
Not perfect but workable.