voron5x with RTCP

OttoJiang

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.

droftarts

@OttoJiang This is @JoergS5 and @dc42's area of expertise, not mine!

Ian

OttoJiang

@droftarts Ok, I got it, sry

droftarts

@OttoJiang No need to apologise, it's just that I am not a programmer.

Ian

JoergS5

@OttoJiang said in voron5x with RTCP:

It should actually move in a curve to keep the nozzle's XYZ coordinates in the workpiece coordinate system constant during the movement.

Hello,

correction of the XYZ, so the nozzle stays at the position when BC changes, is exactly what the code shall do, and it worked in my tests. Please tell me where you mean the error in the code lies.

Your other post, you want constant velocity:
Segmentation works to linearly segment every axis movement/rotation angle. To achieve what you want, you need a change of the segmentation code. First to change is the BC angle change. As an example, to rotate in 10 segments 45 degrees, segmentation is 4.5 degrees for every segment. But needed is something like the Slerp algorithm to divide the BC into a constant velocity. But the code needed is located outside of the kinematics code. Currently angles are simply divided for segmentation, B divided and C divided.

(to be more exact: you cannot make all rotation movements with BC alone, you would need 3 rotation axes. There is an article to approach the correct movement with BC, missing the A axis, with minimal error. The code uses Slerp code also. Slerp is easiest calculated using Quaternions)

To make it worse, if you need true constant velocity, you must know your path and combine the movements of the linear axes with the rotation Slerp-based calcuation and then calculate the segmentation needes of every axis. Calcuation of the exact path needs a lot of mathematical calculation. It is better made in advance by a slicer and sending the single segments as G-Code, instead of letting Duet calculate the segmentation. You need in the range of 10 microseconds for every segment to have an efficient movement. (You want a low millisecond for a movement, with maybe 100 segments). The current code is in this range, but a path movement (including BSpline, Bezier and such) calculation will add time.

The current RRF code is based on time framed calculation: how many steps needs every axis, then put them together into the time frame, then try to send one step signal to as many axes as possible at the same time (Duet sends one signal to multiple motor drivers). You requirement needs separate signal calcuations and step signals for every axis at his own, each one at a given time. It's not impossable, but needs more caculating power.

JoergS5

@OttoJiang

I am confused a bit by your mp4 video file, because the printer behaves exactly as expected. Is the video with your bug code correction?

Maybe you means the Z moving up and down a bit during the movement. This may also be because of Z backlash *) in the construction. How is your Z axis built? It needs to be without backlash. The requirement of Z movement is much higher now than a simple movement in one direction for a 3-axis printer.

*) you will probably know what I mean with backlash. But for other readers: I mean when the Z axis motor changes direction, some steps are lost because there is play in the gear/ball gears/axes with the effect that the opposite movement is later than expected. Some steps are lost. The 3-axis printers don't have backlash because they move only upward and gravity assures that the gear/beal gear are always at one flange of the machanics.

OttoJiang

@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.

OttoJiang

@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

@OttoJiang said in voron5x with RTCP:

This kinematics approximates linear motion by means of segmentation

thank you that you found the solution of a problem I was not aware of.

@dc42 could you please add this code to the GCodes part, maybe with some flag condition for specific kinematics?

OttoJiang

@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

@OttoJiang I don't recommend it, because this kinematics is only one of several ones of robotics kinematics. When they are complete, we can github it.

As an overview please see https://docs.duet3d.com/User_manual/Machine_configuration/Configuring_RepRapFirmware_for_a_Robot_printer with additional documentation pages.