Penta Axis (PAX) support

[xyzdims]

I'm currently coding for Penta Axis (PAX) 5-axis FDM option https://xyzdims.com/2021/02/08/3d-printing-penta-axis-pax-5-axis-printing-option/ on Movement/Kinematics/PAXKinematics.{cpp,h} aka M669 K14 @dc42 and I need some guidance to implement some inverse kinematics needed for MotorStepsToCartesian() & CartesianToMotorSteps(), something like:

matrix4x4 m2 = mtranslate([xoff,yoff,zoff]);
matrix4x4 m1 = mrotate([xrot,0,0]);
matrix4x4 m0 = mrotate([0,0,zrot]);
matrix4x4 m = minvert(m2*m1*m0);  

x = m.elem[0][3], y = m.elem[1][3], z = m.elem[2][3];   // extracting translation part again

It's a bit more complex, but in a nutshell I need a sequence of rotate/translate in different orders, and inverse at the end, and then translation vector extraction and thought doing this more high level with 4x4 matrices.

I'm not versed in C++ and Eclipse but I know C sufficiently, but it's 20+ years gone by since (Perl/Python these days).

It's not clear to me whether basic matrix ops are available in RRF, and which library is available and/or used, it seams Math/Matrix.h provides mainly structural functionality (create & set), and inversion of matrices (GaussJordan reduction), but not multiplication of matrices, correct?

What's the best way to implement mtranslate(), mrotate() and minvert() in RRF?

Cc: @JoergS5 (author of FiveAxisRobotKinematics.cpp)

[JoergS5]

@xyzdims that's an interesting project!

To my knowledge there is no support of those functions in RRF yet. I'll implement 6 axis robot with jordan transformations also and need the functions as well. This will be in 2-3 months from now.

Maybe someone can recommend a library to be added to the RRF library. [I found e.g. https://github.com/bedapisl/matrix_library => this seems to be not the best one]

[dc42]

@xyzdims matrix multiplication could certainly be added to the maths library. However, if the matrices are sparse then it would be more efficient to do the calculations manually, to save a lot of multiplying by 0 or 1 and adding zero terms.

[xyzdims]

@joergs5 I just tried the library you mentioned, but I get many errors when including it, it struggles with ios dependency (within matrix_library/matrix.h by include of<iterator> and <complex> (I didn't find an easy way to resolve it).

@dc42 @JoergS5 I gonna code some matrix helpers for my use case, once things run well and reliable I get back to you and you can look at my code and consider to include something in RRFLibrary/Math.

PS: I spent hours chasing errors reported under Eclipse (Linux), but console reported 0 errors - once I ran Index > Rebuild, the errors under "Problems" disappeared

[JoergS5]

@xyzdims said in Penta Axis (PAX) support:

I just tried the library you mentioned, but I get many errors

Maybe you were missing using namespace std; in the cpp code, because there are std::ios, std::iterator and std::complex standard classes then. But anyway, good luck implementing the matrix methods.

[xyzdims]

@joergs5 I've got my kinematics numerically working with the few helpers based on FixedMatrix<floatc_t,4,4> I mentioned (not yet optimized or so).

Still some stuff to sort out regarding LimitSpeedAndAcceleration() to coordinate different speeds of my axes (they differ quite a lot in speed).

[JoergS5]

This post is deleted!

[xyzdims]

@dc42 I post here and intentionally avoid creating a new topic (if you prefer new topic, I will do so next time).

Since more than a week I struggle with a bug in my new kinematics, and I can't comprehend RRF/Movement/Kinematics stack with DDA in details yet - so I would appreciate a few hints from you.

Problem: my kinematics has 2 additional head rotation axis for FDM, Zrotation (Axis A) and tilt rotation (Axis B); my numeric calculations work, but mechanical it behaves likes this:

• G1 B10: expected: B, Y & Z move; actual result: click/blocked (perhaps too fast) motor movement of B, Y and Z (hardly move at all, missing correct final position)
• G1 Y0.1 B10: B, Y and Z move correctly (right speed, right final position)
• G1 X0.1 B10: B, Y and Z move correctly and X a bit

When I disable B affecting Y & Z within my kinematics, then:

• G1 B10: moves B correctly (not affecting Y & Z)

Presumbly wrong logic at work:

• when a rotational axis moves alone (based on G1 code), but CartesianToMotorSteps() provides motions involving other axes, wrong speed/accel is calculated and performed
• when a rotational axis moves with another axis requested, correct speed/accel is calculated

Any thoughts?

PS: my PAXKinematics::LimitSpeedAndAcceleration() is present but doesn't limit anything, and I'm on 3.3-dev

[JoergS5]

@xyzdims my idea would be you should set a lower speed and accel for B, instead of
M203 A20000 B20000
M201 A800 B800
lower values. M203 B20000 is 333 degree/second, which is quite high*). You can calculate how fast Y and Z must be for this B speed. I expect that the Y and Z speed is too high, failing to reach the coordinated BYZ speeds.

*) with high I don't mean the rotation speed, but the rotation of the nozzle plus the distance of the nozzle to the hinge axis produce a YZ distance which may be too high. I didn't check your nozzle-hinge-distance, but the more, the worse. I cannot find the information how you set the nozzle-hinge distance. Is it by parameter in config?

Roughly calculated, if nozzle-hinge distance is 5 cm, and 10 degree means 30 ms for rotation (333 degrees/s). Starting vertical nozzle, roughly all movement goes into Y movement (sin(10*3.14/180)*5 = 0.868 cm), which is 290 mm/s for Y movement. This is not reachable by config restriction for Y, if config Y10000 = 166 mm/s is correct. But it's only a rough calculation, I ignored accel of B and Y eg.

My understanding of firmware movement code, I hope my understanding is correct:
G1 Y0.1 B10 slows down B, because Y restricts the total time limit for the movement. The slowest actuator sets the total time needed, and step generation of all actuators are calculated according to this total time. In most cases the slowest actuator is the extruder speed (not here, but when 3d printing).

[xyzdims]

Some debugging infos (M111 S1 P4 + P6), more useful for you @dc42 :
G1 B20 (fails to perform mechanically):

pr 1 ts=43851297 DDA: start=[0.000000 0.000000 0.000000 0.000000 20.000000] end=[0.000000 0.000000 0.000000 0.000000 20.000000] s=0.000000 vec=[0.000000 0.000000 0.000000 0.000000 20.000000]
a=40.000000 d=nan reqv=4.166667 startv=0.000000 topv=0.000000 endv=0.000000
cks=0 sstcda=0 tstcddpdsc=0 exac=0
DMB: dir=F steps=45 next=1 rev=46 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0
DMZ: dir=B steps=1085 next=1 rev=1086 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0
DMY: dir=B steps=1539 next=1 rev=1540 interval=0 2dtstc2diva=nan
accelStopStep=1 decelStartStep=1 2c2mmsda=0.00 2c2mmsdd=nan
mmPerStepTimesCdivtopSpeed=nan fmsdmtstdca2=0.00 cc=0 acc=0

[ At the first glance it's the first 'd=nan'which raises my attention ]

and debugging info from my kinematics (appears before the above debugging output), whereas ma machinePos mt motorPos:

; CartesianToMotorSteps: zoff=45.000 xm=0.000 ym=0.000 zm=0.000 am=0.000 bm=20.000
;tool->machine(1): x=0.000 y=15.391 z=-42.286
;tool->machine(2): x=0.000 y=15.391 z=2.714
;=== ma.x=0.000 y=-15.391 z=-2.714
;    mt.x=0 y=-1539 z=-1085
; 5 numVisibleAxes
; axis #0: ma.pos 0.00 (100.00stp/mm) mt.pos 0
; axis #1: ma.pos 0.00 (100.00stp/mm) mt.pos -1539
; axis #2: ma.pos 0.00 (400.00stp/mm) mt.pos -1085
; axis #3: ma.pos 0.00 (2.25stp/mm) mt.pos 0
; axis #4: ma.pos 20.00 (2.25stp/mm) mt.pos 45

As comparison, performing G1 X0.1 B20 (succeeds to perform mechanically):

pr 1 ts=323555603 DDA: start=[0.000000 0.000000 0.000000 0.000000 -0.000000] end=[0.100000 0.000000 0.000000 0.000000 20.000000] s=0.100000 vec=[1.000000 0.000000 0.000000 0.000000 200.000000]
a=4.000000 d=4.000000 reqv=0.416667 startv=0.000000 topv=0.416667 endv=0.000000
cks=258125 sstcda=0 tstcddpdsc=258125 exac=39062
DMY: dir=B steps=1539 next=1 rev=1540 interval=4274 2dtstc2diva=28124999680.00
accelStopStep=334 decelStartStep=1206 2c2mmsda=18274854.00 2c2mmsdd=18274854.00
mmPerStepTimesCdivtopSpeed=116.96 fmsdmtstdca2=0.00 cc=0 acc=0
DMZ: dir=B steps=1085 next=1 rev=1086 interval=5091 2dtstc2diva=28124999680.00
accelStopStep=236 decelStartStep=850 2c2mmsda=25921658.00 2c2mmsdd=25921658.00
mmPerStepTimesCdivtopSpeed=165.90 fmsdmtstdca2=0.00 cc=0 acc=0
DMB: dir=F steps=45 next=1 rev=46 interval=25000 2dtstc2diva=28124999680.00
accelStopStep=10 decelStartStep=36 2c2mmsda=625000000.00 2c2mmsdd=625000000.00
mmPerStepTimesCdivtopSpeed=4000.00 fmsdmtstdca2=0.00 cc=0 acc=0
DMX: dir=F steps=10 next=1 rev=11 interval=53033 2dtstc2diva=28124999680.00
accelStopStep=3 decelStartStep=8 2c2mmsda=2812499968.00 2c2mmsdd=2812499968.00
mmPerStepTimesCdivtopSpeed=18000.00 fmsdmtstdca2=0.00 cc=0 acc=0

and my debugging (also comes before):

; CartesianToMotorSteps: zoff=45.000 xm=0.100 ym=0.000 zm=0.000 am=0.000 bm=20.000
;tool->machine(1): x=0.000 y=15.391 z=-42.286
;tool->machine(2): x=0.000 y=15.391 z=2.714
;=== ma.x=0.100 y=-15.391 z=-2.714
;    mt.x=10 y=-1539 z=-1085
; 5 numVisibleAxes
; axis #0: ma.pos 0.10 (100.00stp/mm) mt.pos 10
; axis #1: ma.pos 0.00 (100.00stp/mm) mt.pos -1539
; axis #2: ma.pos 0.00 (400.00stp/mm) mt.pos -1085
; axis #3: ma.pos 0.00 (2.25stp/mm) mt.pos 0
; axis #4: ma.pos 20.00 (2.25stp/mm) mt.pos 45

[xyzdims]

@joergs5 thanks for participating

M203 B20000 is 333 degree/second, which is quite high*)

RRF all speeds are mm or degrees per minutes. Also, G1 A20 performs ok (doesn't not involve X/Y/Z if B=0).

I have lowered it earlier to M203 A10000 B10000 (the info on my blog isn't up-to-date as I change it a lot), but as you perhaps read on my above experience, it's not so much tuning min/max speed the problem as such, the click/blocked motor indicates way out of range speed or acel/decel, there is miscalculation somewhere on DDA/DDARing level, which my debug info I posted today confirms or hints to. I currently try to comprehend Move/DDA/DDARing functionality better, perhaps I'm missing an important config which leads to the d=nan (deceleration) line (see my other post on this thread).

[dc42]

@xyzdims unfortunately I won't have time to look at this in detail for the next couple of weeks. But the following may be useful:

• Acceleration and speed limits set by M201 and M203 apply to the movements of axes as specified on the command line. So if e.g. you command X to move and your kinematics causes B to move as well, the acceleration and speed B won't be limited by the M201/M203 B values, only by the M201/203 X values and the ration of B movement to X movement.
• If you get NaNs in the calculations, this normally means that somewhere you have taken the square root of a negative number, or taken an arcsin or arccos of a number outside the range -1.0..1.0.
• This bit of your debug looks odd:

pr 1 ts=43851297 DDA: start=[0.000000 0.000000 0.000000 0.000000 20.000000] end=[0.000000 0.000000 0.000000 0.000000 20.000000] s=0.000000 vec=[0.000000 0.000000 0.000000 0.000000 20.000000]
a=40.000000 d=nan reqv=4.166667 startv=0.000000 topv=0.000000 endv=0.000000
cks=0 sstcda=0 tstcddpdsc=0 exac=0

Assuming you created extra axes A B only in that order, this means that your kinematics command a move of total distance NaN after segmentation, and B was moving 20 times as much as the distance specified in the G1 command. That's not right, only axes that are moving implicitly should have have a 'vec' component greater than 1. You need to find out where that NaN is coming from. Check that you haven't accidentally set the segmentation distance to zero.

[dc42]

PS - you might want to try the same move using regulator Cartesian kinematics + your A and B axes, to check that the code outside your kinematics is behaving properly in the same configuration.

[xyzdims]

The "pr" comes from void DDA::Prepare() in DDA.cpp, there d or decelaration is NAN.

G1 B20: DDA "pr": a=40.0 (aceleration), d=NAN (deceleration), s=0.0 (distance)

G1 X0.1 B20: DDA "pr": a=4.0, d=4.0, s=0.1

I need to dive deeper into the DDA.cpp to find out, I suspect the main problem could be s=0.0; just after if(delta!=0) in DDA::Prepare() the debug is printed out with tag "pr", it determined there is change, but distance is 0 (=> contradiction), and if any value is divided by s (=0.0) before we get NAN.

[xyzdims]

@dc42 thanks for the hints, I assumed you must be super busy I try to proceed by myself and with @JoergS5 input and share my findings.

[xyzdims]

So, DDA.cpp: DDA::Prepare() calls https://github.com/Duet3D/RepRapFirmware/blob/3.3-dev/src/Movement/DriveMovement.cpp#L59 PrepareCartesianAxis() just before the debug, and there

const float stepsPerMm = (float)totalSteps/dda.totalDistance;

and dda.totalDistance is 0.0 . . . and other calculations follow about acceleration / deceleration, there the NAN is created.

The problem starts somewhere in DDA.cpp DDA::InitStandardMove(), there totalDistance is calculated, which should be non-zero and it looks it is 0.0 there too.

        if (linearAxesMoving)
	{
		// There is some linear axis movement, so normalise the direction vector so that the total linear movement has unit length and 'totalDistance' is the linear distance moved.
		// This means that the user gets the feed rate that he asked for. It also makes the delta calculations simpler.
		// First do the bed tilt compensation for deltas.
		directionVector[Z_AXIS] += (directionVector[X_AXIS] * k.GetTiltCorrection(X_AXIS)) + (directionVector[Y_AXIS] * k.GetTiltCorrection(Y_AXIS));
		totalDistance = NormaliseLinearMotion(reprap.GetPlatform().GetLinearAxes());
		debugPrintf("DDA::InitStandardMove.1: %f totalDistance\n",totalDistance);
	}
	else if (rotationalAxesMoving)
	{
		// Some axes are moving, but not axes that X or Y are mapped to. Normalise the movement to the vector sum of the axes that are moving.
		totalDistance = Normalise(directionVector, reprap.GetPlatform().GetRotationalAxes());
	       debugPrintf("DDA::InitStandardMove.2: %f totalDistance\n",totalDistance);
	}
        else
	{
		// Extruder-only movement. Normalise so that the magnitude is the total absolute movement. This gives the correct feed rate for mixing extruders.
		totalDistance = 0.0;
		for (size_t d = 0; d < MaxAxesPlusExtruders; d++)
		{
			totalDistance += fabsf(directionVector[d]);
		}
		if (totalDistance > 0.0)		// should always be true
		{
			Scale(directionVector, 1.0/totalDistance);
		}
	}
	debugPrintf("DDA::InitStandardMove.3: %f totalDistance\n",totalDistance);

linearAxesMoving and rotationalAxesMoving are both true when I do G1 B20, so which gives strange debug results:

DDA::InitStandardMove.1: 0.000000 totalDistance
DDA::InitStandardMove.3: 0.000000 totalDistance

As reminder: G1 B20, causing Y, Z and B motors to move by request ofPAXKinematics::CartesianToMotorSteps().

That both linearAxesMoving and rotationalAxesMoving are true makes sense, given they mean motor axes.

I think there is a bug in the logic of the code I quoted above:

• if(linearAxesMoving) code block needs to consider rotationalAxesMoving case too, it's both, not "either or" as with else if(rotationalAxesMoving)

I disabled if(0 && linearAxesMoving) case for G1 B20, and I had correct mechanical movement! [Update: https://www.youtube.com/watch?v=2x_rOThE_Ns doing G1 B45 and G1 B0 again, the nozzle tip is supposed to stay in place in relation to bed (hence Y, Z and B movement) ].

@dc42 Let me know if there is someone else I should address this in case you don't have the time/capacity. I think we are close to resolve this.

[dc42]

@xyzdims thanks for looking into this. I think the bug is that if you have declared B to be a rotational axis (the default), then linearAxesMoving should not be set for a G1 B20 move. It should be set if any only if there is motion of the tool head in Cartesian space. The fact that your kinematics has to move X, Y or Z when moving B should not cause linearAxesMoving to be set.

Looking into this some more, I think the linear or rotational axis moving flag should be set as moving if positionDelta is nonzero, not if delta is nonzero as at present.

[xyzdims]

@dc42 doMotorMapping is true in DDA::InitStandardMove(); not sure what the flag actually means, I inserted some more debugPrintf in the relevant loop:

			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				debugPrintf("axis #%d: posN %d -> endP %d => delta=%d\n",drive,positionNow[drive],endPoint[drive],delta);
				const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
				debugPrintf("   positionDelta=%f\n",positionDelta);
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}

G1 B20 requested:

doMotorMapping=1, numVisibleAxes=5
axis #0: posN 0 -> endP 0 => delta=0
   positionDelta=0.000000
axis #1: posN 0 -> endP -1539 => delta=-1539
   positionDelta=0.000000
axis #2: posN 0 -> endP -1085 => delta=-1085
   positionDelta=0.000000
axis #3: posN 0 -> endP 0 => delta=0
   positionDelta=0.000000
axis #4: posN 0 -> endP 45 => delta=45
   positionDelta=20.000000
linearAxesMoving=1, rotationAxesMoving=1
DDA::InitStandardMove.1: totalDistance=0.000000
DDA::InitStandardMove.3: totalDistance=0.000000

Hope this helps.

[dc42]

@xyzdims this is the current code (I am looking at 3.4beta1 so it might be slightly different in 3.3):

	for (size_t drive = 0; drive < MaxAxesPlusExtruders; drive++)
	{
		accelerations[drive] = normalAccelerations[drive];
		endCoordinates[drive] = nextMove.coords[drive];

		if (drive < numVisibleAxes)
		{
			int32_t delta;
			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}
			else
			{
				// Raw motor move on a visible axis
				endPoint[drive] = Move::MotorMovementToSteps(drive, nextMove.coords[drive]);
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = (float)delta/reprap.GetPlatform().DriveStepsPerUnit(drive);
			}

			if (delta != 0)
			{
#if 0	// debug only
				stepsRequested[drive] += labs(delta);
#endif
				if (reprap.GetPlatform().IsAxisRotational(drive))
				{
					rotationalAxesMoving = true;
				}
				else
				{
					linearAxesMoving = true;
				}
			}
		}
		else if (LogicalDriveToExtruder(drive) < reprap.GetGCodes().GetNumExtruders())

This is what I think it should be:

	for (size_t drive = 0; drive < MaxAxesPlusExtruders; drive++)
	{
		accelerations[drive] = normalAccelerations[drive];
		endCoordinates[drive] = nextMove.coords[drive];

		if (drive < numVisibleAxes)
		{
			const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
                        if (positionDelta != 0.0)
                        {
				if (reprap.GetPlatform().IsAxisRotational(drive))
				{
					rotationalAxesMoving = true;
				}
				else
				{
					linearAxesMoving = true;
				}
                        }
			int32_t delta;
			if (doMotorMapping)
			{
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = positionDelta;
				if (positionDelta != 0.0 && (Tool::GetXAxes(nextMove.tool).IsBitSet(drive) || Tool::GetYAxes(nextMove.tool).IsBitSet(drive)))
				{
					flags.xyMoving = true;				// this move has XY movement in user space, before axis were mapped
				}
			}
			else
			{
				// Raw motor move on a visible axis
				endPoint[drive] = Move::MotorMovementToSteps(drive, nextMove.coords[drive]);
				delta = endPoint[drive] - positionNow[drive];
				directionVector[drive] = (float)delta/reprap.GetPlatform().DriveStepsPerUnit(drive);
			}

#if 0	// debug only
			if (delta != 0)
			{
				stepsRequested[drive] += labs(delta);
			}
#endif
		}
		else if (LogicalDriveToExtruder(drive) < reprap.GetGCodes().GetNumExtruders())

[xyzdims]

@dc42 I copied it, thanks! It works but give me a day or so to confirm some edge cases as well.