Coupled polar kinematics

achrn

I'm contemplating using RRF as motion control system on what is essentially a polar machine , but the mechanics is such that the radial position is slightly coupled to the angular position. This because radial position is driven by a shaft coaxial to the turntable axis, so rotating the turntable while the radial drive shaft is stationary introduces a rotation between the turntable rotating side (where the radial mechanism is positioned) and that shaft.

In steps terms, 40 steps of the turntable stepper will move the radial position a distance equivalent to one step of the radial stepper. (Roughly, but around that sort of number, I think.)

The reverse is not coupled - if you drive the radial position stepper while the angular position is stationary the 'print head' would move radially but the turntable would not rotate.

I think the serial scara kinematics already something similar to this function - the M669 'crosstalk factors'.

I've seen: https://duet3d.dozuki.com/Wiki/ConfiguringRepRapFirmwarePolarPrinter
and also: https://github.com/Duet3D/RepRapFirmware/wiki/Adding-New-Kinematics

I've had a little rummage in the source (specifically PolarKinematics.cpp) and it looks to me like this is a relatively simple modification. At least, the geometrical maths seems simple, the bit that actually I don't understand is how to convey some C parameters into the PolarKinematics functions.

Is that right, or have I described something really complex?
I don't want to fall into a 'I don't know how to do it but it seems fairly simple' trap.

dc42

@achrn I think you have the right idea. The place to implement a new coupling parameter is in the Configure function which handles the M669 command.

achrn

@dc42 Thanks. I've now done this by modifying the existing polar kinematics rather than adding a new one. I've added a C parameter to M669 e.g. M669 K7 R300 F60 A60 C0.11111 where the C parameter is specified in millimetres per degree, and equals the distance by which radial (X) position changes when turntable (Y) changes by one degree. Positive C indicates that anticlockwise rotation of the turntable increases the radial position (if not corrected).

This seemed quite straightforward, and seems to work, though I haven't actually built the mechanics yet - I'm just watching the behaviour of steppers on my desk. I've modified a v3.3 firmware, and it's actually @gloomyandy's port of RRF to STM boards (I'm using a Mellow Fly E3 Pro). (Big thank you to @gloomyandy for this).

Reading the source, there's one aspect that seems redundant to me: in PolarKinematics::CartesianToMotorSteps it has
motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);
It seems to me that any atan2 worth its salt would cope with (0,0) as input and return something correct (e.g. 0.0) in which case the conditional here is redundant. However, I haven't tracked down where the atan2f is defined to verify that it does behave. This is relevant because my code is simpler if I can calculate motorPos[1] before calculating motorPos[0] (because [1] changes [0] but [0] does not affect [1]).

In case anyone else is interested, and with the caveat that I haven't tested properly (as noted, just bare steppers on my desk so far):

In PolarKinematics.h I add a private variable to hold crosstalk factor:

@@ -48,6 +48,7 @@
 
 	float minRadius, maxRadius, homedRadius;
 	float maxTurntableSpeed, maxTurntableAcceleration;
+	float crosstalk;
 
 	float minRadiusSquared, maxRadiusSquared;
 };

In PolarKinematics.cpp add crosstalk initialisation to constructor:

@@ -39,7 +39,8 @@
 PolarKinematics::PolarKinematics() noexcept
 	: Kinematics(KinematicsType::polar, SegmentationType(true, false, false)),
 	  minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0),
-	  maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration)
+	  maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration),
+	  crosstalk(0.0)
 {
 	Recalc();
 }

In PolarKinematics::Configure add initialisation of crosstalk value and reporting of the crosstalk value:

@@ -90,15 +91,17 @@
 		gb.TryGetFValue('A', maxTurntableAcceleration, seenNonGeometry);
 		gb.TryGetFValue('F', maxTurntableSpeed, seenNonGeometry);
 
+		gb.TryGetFValue('C', crosstalk, seen);
+
 		if (seen)
 		{
 			Recalc();
 		}
 		else if (!seenNonGeometry && !gb.Seen('K'))
 		{
-			reply.printf("Kinematics is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f",
+			reply.printf("Kinematics is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f, rotn to radius crosstalk %.3fmm/deg",
 							(double)minRadius, (double)maxRadius, (double)homedRadius,
-							(int)GetSegmentsPerSecond(), (double)GetMinSegmentLength());
+							(int)GetSegmentsPerSecond(), (double)GetMinSegmentLength(), (double)crosstalk);
 		}
 		return seen;
 	}

In PolarKinematics::CartesianToMotorSteps add crosstalk effect (and change the order of calculation, as discussed above):

@@ -116,8 +119,8 @@
 // Return true if successful, false if we were unable to convert
 bool PolarKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept
 {
-	motorPos[0] = lrintf(fastSqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]);
-	motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);
+	motorPos[1] = lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);
+	motorPos[0] = lrintf(fastSqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0] - motorPos[1] / stepsPerMm[1] * crosstalk * stepsPerMm[0]);
 
 	// Transform remaining axes linearly
 	for (size_t axis = Z_AXIS; axis < numVisibleAxes; ++axis)

In PolarKinematics::MotorStepsToCartesian add crosstalk effect:

@@ -135,7 +138,7 @@
 void PolarKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept
 {
 	const float angle = (motorPos[1] * DegreesToRadians)/stepsPerMm[1];
-	const float radius = (float)motorPos[0]/stepsPerMm[0];
+	const float radius = (float)motorPos[0]/stepsPerMm[0] + (float)motorPos[1]/stepsPerMm[1]*crosstalk;
 	machinePos[0] = radius * cosf(angle);
 	machinePos[1] = radius * sinf(angle);

dc42

@achrn said in Coupled polar kinematics:

Reading the source, there's one aspect that seems redundant to me: in PolarKinematics::CartesianToMotorSteps it has
motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);
It seems to me that any atan2 worth its salt would cope with (0,0) as input and return something correct (e.g. 0.0) in which case the conditional here is redundant.

The full snippet is:

    motorPos[0] = lrintf(fastSqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]);
    motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);

If motorPos[0] == 0.0 then assuming stepsPerMm[0] is nonzero, it can only mean that machinePos[0] and machinePos[1] are both 0, in which atan2f may be undefined and raise a domain error, depending on the implementation.

achrn

achrn said (inter alia) in Coupled polar kinematics:

In PolarKinematics::CartesianToMotorSteps add crosstalk effect (and change the order of calculation, as discussed above):

This (and MotorStepsToCartesian) turns out to be not as simple as I hoped, and with a little thought it becomes apparent why - with the coupling between X and Y axes, there is no 1:1 mapping between the machine target position and a set of motor positions. With radial position controlled by a shaft coaxial to the turntable, the 'motorPos' X axis (radial distance) depends upon whether that position is reached turning clockwise or anticlockwise. Mostly this works out OK, but my code (up the thread) doesn't work when you cross the line where the atan2 function swaps (i.e. +/- 180 degrees).

That is, if I'm at position [-100,1] and want to move to [-100,-1], the correct path is obviously (to a human) 2mm long and features a small change in angle and no change in radius, but the firmware CartesianToMotorSteps() function has no concept of path, and moving from 100 radius at 179.9 degrees to 100 radius at -179.9 degrees, is 359.8 degrees of rotation, which (with the coupling I have) would require a pretty big shift in radial motor position. The firmware does know the angle is a continuous rotation axis and handles that OK, but my code doesn't know that this apparent 359.8 degree change is not actually a big rotation, and tries to introduce a big and instantaneous change to radial position as it crosses the line.

I haven't figured out a solution to this. I'm slightly nervous that a rigorous solution doesn't exist (because of the structure of the kinematics functions) - I think that might need a CartesianToMotorSteps that has as input not only where it needs to be but also where it's coming from. However, I need to delve more into how the firmware handles a continuous rotation axis and factor something similar into the radial axis (i.e. the code does know that it can rotate from -179.9 degrees to 179.9 degrees by rotating -0.2 degrees not +359.8 degrees, I need to tell it that while actually rotating +359.8 degrees requires a large change in radial position motor, this move doesn't need that change).

So mostly I'm just posting to close off the possibility that anyone searching in future finds my previous post and believes it - don't believe it, it's more complex than that suggests.

JoergS5

@achrn you could try to avoid big angle changes by storing the planned complete move and make sure when calcuation of the segments occur, that there are no big angle changes. Your problem is similar to kinematics where we have multiple inverse solutions for a given forward kinematics. Then the solution can be taken which is nearest to the segmented moves before on the path.

To avoid changes in separate firmware code, I stored the path when LimitPosition is called, which is called at the beginning of a new move. LimitPosition has initialCoords and finalCoords for G0/G1 moves and finalCoords for G2/G2 moves. For G2/G3 the finalCoords values of the move before should be stored as new starting position.

The motorPos are int32_t values, so there are some float-int conversions in the code above which I would avoid.

achrn

@joergs5 said in Coupled polar kinematics:

make sure when calcuation of the segments occur, that there are no big angle changes.

I think this is the bit I need to find in the code. Since it is segmenting the path, I should be able to identify when something is wrapping around, i.e. in a segmented path, there shouldn't be a segment where angle changes by nearly 360 degrees. (But there could be a 180 degree change - e.g. from [0.99,0.01] to [-0.99,-0.01] is a 2mm long segment that features a 180 degree rotation).

Thanks for the pointers.

JoergS5

@achrn
I think you have to change the CartesianToMotorSteps motorPos values when the described situation arises. The solution is, to set motorPos[1] to a value less (or more) one full rotation value. For analysis, the M114 Count values can be used, which are the motorPos values.
The minimum and maximum values of motorPos should probably be limited to avoid wire or filament break (wires of heated bed e. g.).

achrn

@joergs5 said in Coupled polar kinematics:

The minimum and maximum values of motorPos should probably be limited to avoid wire or filament break (wires of heated bed e. g.).

No wires - the radial and angular position are of the effector, and bed is stationary. There's also no material fed across the movement interface - it's effectively a pen plotter. Thus it is a genuinely continuous rotation bed.

(The first limit is motor steps counter stored in a 32 bit variable, which gives me 'only' about 26 hours of continuous one-way rotation. I don't anticipate exceding that limit, however dc42 advises that G92 will reset the steps counter, so some gcode can overcome that 'limit'.)

rjenkinsgb

@achrn said in Coupled polar kinematics:

the radial and angular position are of the effector, and bed is stationary.

How about linking an encoder to the rotary axis - eg. via a 1:1 timing belt drive - and using closed loop control for the radial position, with that encoder as the feedback?

It should cause the radial motor to track and inherently compensate the angular motion?

JoergS5

@achrn said in Coupled polar kinematics:

No wires - the radial and angular position are of the effector, and bed is stationary.

So there will be no problem to implement the proposed change to the code.

One could use brushes (or pogo pins) to implement continuous heated roting plates if needed.

JoergS5

@achrn I think the reason is the jump of the atan2 function at 0 from pi (180 degrees) to -pi if x < 0, please see https://en.m.wikipedia.org/wiki/Atan2. x=0 is not defined for atan2.

I would propose to store an absolute counter (how many full rotations are made) and caching the motorPos of last move, so instead of the big change, a small one with one rotation more/less can be made. The absolute counter is not important for your indefinite rotation plate, but for those who have to rewind. The 180 degree positions can be calculated from the M92 values, because M92*180 (and *(360+180), *(-360+180) etc.) are the 180 degree positions.

achrn

@rjenkinsgb said in Coupled polar kinematics:

How about linking an encoder to the rotary axis - eg. via a 1:1 timing belt drive - and using closed loop control for the radial position, with that encoder as the feedback?

It should cause the radial motor to track and inherently compensate the angular motion?

I can see how it would work (in principle), but I fear it would be more complicated than getting the code to determine when it is jumping and doesn't need the implied radial position adjustment. Also, I'm endeavouring to avoid wires across the rotating interface, and the encoder tracking radial position is the 'otehr side' of that interface, so would need to signal across it. At the moment I'm using stall detection to home the radial axis and it's purely coaxial mechanical elements across the rotating interface.

@joergs5 said in Coupled polar kinematics:

I think the reason is the jump of the atan2 function at 0 from pi (180 degrees) to -pi if x < 0, please see https://en.m.wikipedia.org/wiki/Atan2. x=0 is not defined for atan2.

Yes, I'm sure that's the cause of the jump (as the mechanism moves through that location the stepper driving the radial position chirps / screams / jumps - I think it basically just gets hit with an instantaneous demand to turn one whole revolution because it 'thinks' the angular position has turned one whole revolution). I need to look at the code more closely, or more accurately, look at more of the code (than just the two functions CartesianToMotorSteps and MotorStepsToCartesian) to work it out.

Thanks all.

rjenkinsgb

@achrn said in Coupled polar kinematics:

Also, I'm endeavouring to avoid wires across the rotating interface, and the encoder tracking radial position is the 'otehr side' of that interface

?? The encoder would be fixed, probably near the rotation drive motor - it's to measure the rotary position, not radial, even though it's to add in to the radial drive position to counter the rotational influence.

Another alternative setup would be to use a differential (ie. like a vehicle axle type) to combine the rotation with the radial feed.

That's done with machine tools (horizontal borers) to control the radial position of the tool slide on a rotary facing heat, as that otherwise has the same problem.

Other complications & hardware vs software are a different matter and are all down to you, I'm just throwing in ideas..

achrn

@rjenkinsgb said in Coupled polar kinematics:

@achrn said in Coupled polar kinematics:

Also, I'm endeavouring to avoid wires across the rotating interface, and the encoder tracking radial position is the 'otehr side' of that interface

?? The encoder would be fixed, probably near the rotation drive motor - it's to measure the rotary position, not radial, even though it's to add in to the radial drive position to counter the rotational influence.

Sorry, yes you're right. I had it swapped round in my mind.

I also hadn't thought about a purely mechanical solution - that has some attraction. Thanks.

dc42

@achrn said in Coupled polar kinematics:

With radial position controlled by a shaft coaxial to the turntable, the 'motorPos' X axis (radial distance) depends upon whether that position is reached turning clockwise or anticlockwise. Mostly this works out OK, but my code (up the thread) doesn't work when you cross the line where the atan2 function swaps (i.e. +/- 180 degrees).

To me, the obvious solution is to disable continuous rotation when the crosstalk factor is nonzero. However, that means that you won't be able to print in a small sector of the bed.

Coupled polar kinematics

Robert J.

Robert J.