more flexible EmcPose representation

[ArcEye]

Issue by mhaberler Mon Apr 21 23:27:56 2014 Originally opened as https://github.com/machinekit/machinekit/issues/151

---

Currently the internal representation of axis positions looks like so:

typedef struct {
    double x, y, z;     /* this.x, etc. */
} PmCartesian;
typedef struct EmcPose {
    PmCartesian tran;
    double a, b, c;
    double u, v, w;
} EmcPose;

This has no provision for NULL axis values (not 0.0 but value not present).

There is no indication if the values apply to joints, or axis (pre/post kinematics, joint representation really just being a special identity kins).

Also, this is rather irregular - the benefit of the PmCartesian substructure is unclear, and the discrete-scalar representation makes iteration over axis values awkward.

While I'm not suggesting to replace this fundamental type everywhere right away, there are work items which permit experimentation with a better representaion, like preview drivers.

A future representation IMO should fulfill these requirements:

• axis are held in a sparse-array like representation
• a kinematics type indicator should be present
• a good match on protobuf fundamental types is desirable

problem: define a more flexible representation for axis values.

[ArcEye]

Comment by jcoffland Mon Apr 21 23:58:40 2014

---

My two cents: I would argue for a simple (KISS) solution. E.g.:

typedef struct {
  double x, y, z;
} Point;

typedef struct {
  Point xyz, abc, uvw;
} Pose;

I don't see why you need a kinematics type indicator. It should be obvious from the context. Adding this would be redundant information. A value of NAN or max double could work for null. I'm not sure how well that maps to protobuf.

[ArcEye]

Comment by jcoffland Mon Apr 21 23:59:24 2014

---

Or this:

typedef struct {
  double p1, p2, p3;
} Point;

[ArcEye]

Comment by mhaberler Tue Apr 22 00:22:53 2014

---

kinematics type indicator: that already is a problem, for instance jog commands.

Jogs are possible in several kins (joint and axis jogs), and there's no good reason why jogging several select axes at the same time should be impossible - that's where the NULL values would be handy. I cant see how you express this in this notation.

In a nutshell, I'm suggesting a Pose should be typed with a kins (also relevant for the comparison operator).

[ArcEye]

Comment by jcoffland Tue Apr 22 02:01:53 2014

---

NAN (not a number) is a value in IEEE 754 floating point format which is what is used on most systems. NAN could be used as NULL for this purpose. It would simplify things because this is basically already implemented for you.

Regarding the kinematic type, I'm really not familiar with the LinuxCNC code.

[ArcEye]

Comment by micges Tue Apr 22 07:54:55 2014

---

I think we should have both representations possible both pose.x pose.y and pose[0] pose[1]. When complex matrix math is going on then it could use .x and for iterations and simple things pose[n].

[ArcEye]

Comment by mhaberler Tue Apr 22 08:31:49 2014

---

protobuf does support the notion of optional fields, so at the transport level this would not be an issue - no need to repurpose NaN; example:

// experimental axis/joint representation
enum KinematicsType {
    KT_JOINT        = 1;
    KT_TRIVKINS     = 2;
    KT_DELTA        = 3;
    // and so forth
}

message Position {
    required KinematicsType kins = 1 [ default = KT_JOINT ];

    optional double x0 = 2;
    optional double x1 = 3;
    optional double x2 = 4;
    optional double x3 = 5;
    optional double x4 = 6;
    optional double x5 = 7;
    optional double x6 = 8;
    optional double x7 = 9;
    optional double x8 = 10;
    optional double x9 = 11;
    // alternative representation:
    repeated double x = 20;
    required fixeds32  axismap = 21; // bitmap of axis values present 
}

library operations would suggest the internal data structures would use a double[] ref as parameter (I dont see the upside of a dual data layout - PmCartesian ops would just operate on values 0..2)

[ArcEye]

Comment by micges Tue Apr 22 09:06:50 2014

---

I was only thinking about source code readability.

[ArcEye]

Comment by mhaberler Tue Apr 22 09:08:00 2014

---

any other comparable package so we can compare notes what they do? I'm not aware of any

[ArcEye]

Comment by robEllenberg Tue Apr 22 15:46:31 2014

---

I agree that the existing representation is awkward to use. My personal list of gripes:

1. The combination of PmCartesian for XYZ and simple doubles for everything else.
2. There isn't a simple way to iterate over the axes due to the above representation.
3. There are currently 4 different formats for storing axis / joint data in use at various points throughout LinuxCNC (individual doubles in the interpreter, CANON_POSITION in canon, struct pt in canon, and EmcPose in motion).

It seems like we could solve all of these issues with an array-based representation to match Michael's message format, something like:

struct Position {
    double axis[9];
    int mask;
    KinematicsType kins;
}

The upside to this representation is that it's easy to do "bulk" operations, especially for kinematics and vector-like operations. Referencing a particular axis is a bit more complicated, since you need to type mypose.axis[X] vs. mypose.x. However, I'd argue that you very rarely need to refer to an explicit axis this way at the kinematics / motion module level.

Dealing with the bitmask will be tricky, since it requires an extra check for any basic operation. Still, this extra complexity should be mostly hidden by helper functions for arithmetic, copying etc. With a small library of these functions, the rest of the code shouldn't have to deal directly with the mask. Of course, a good library also makes dealing with EmcPose a bit less painful (look at src/emc/nml_intf/emcpose.c in the circular-blend-arc-rc3 branch for an example).

This structure could potentially replace CANON_POSITION for the same reasons. A significant fraction of canon is these ugly blocks of 9 lines explicitly doing simple operations. Defining some operators and helper functions with this new structure could cut the source code size of the canon layer in half.

[ArcEye]

Comment by mhaberler Tue Apr 22 15:56:07 2014

---

The 'natural' representation in protobuf would likely be so:

message Axis {
     uint32  num = 1; 
     double  pos = 2;
}
message Position {
    required KinematicsType kins = 1 [ default = KT_JOINT ];
    // repeating group of self-identifying axes,
    // allowing for NULL axes
    repeated Axis axis = 2; 
}

This is slow on indexed access though.

But since library code is affected I agree with Rob that the array/mask representation is likely the better choice

Actually this is a sparse array, and maybe perusing other code and literature for sparse array representation could unearth reusable pieces (numpy anyone ?)

[ArcEye]

Comment by cdsteinkuehler Tue Apr 22 16:07:23 2014

---

On 4/22/2014 10:56 AM, Michael Haberler wrote:

The 'natural' representation in protobuf would likely be so:

message Axis {
     uint32  num = 1; 
     double  pos = 2;
}
message Position {
    required KinematicsType kins = 1 [ default = KT_JOINT ];
    // repeating group of self-identifying axes,
    // allowing for NULL axes
    repeated Axis axis = 2; 
}

I would strongly suggest any changes to this fundamental type take into account the option for non-CNC machines. That means the option for arbitrary numbers of axis and multiple kinematic environments. For instance, what if I wanted to control a robot with two 10-joint arms and six 3-joint legs? IMHO that shouldn't be made impossible by any choices we make today.

I'm not saying everything has to be implemented today, but let's start thinking Machinekit (ie: outside the LinuxCNC box).

I like the idea of sparse arrays if the array bound is not hard-coded (so you could support an arbitrary number of joints). As a plus, this should make the typical case of 3 or 4 axis more efficient.

Charles Steinkuehler charles@steinkuehler.net

[ArcEye]

Comment by mhaberler Tue Apr 22 16:24:28 2014

---

yeah, I have this multiple-kins thing in the back of my head - right now, motion works with a single kins as per halfile, and lots of special casing for joint mode.

however, I dont see any distinction between joint mode, and an identity kins (anybody else does?)- so assuming commands had a kins id attached, motion could choose at runtime which kins apply for the current move - all this insane mode-switching-and-flushing-the-motion-queue-when-switching can go AFAICT

the same goes for the interpreter - that is single kins only which is why the interp cant move in joint mode - why? loading and switching kins would be possible there just alike. The XYZABCUVW words would be overloaded (meaning as per current kins), but I think the upside is there; for instance no good reason the interp cant do homing as an NGC procedure.

[ArcEye]

Comment by robEllenberg Tue Apr 22 17:04:51 2014

---

My complaint about a sparse array as the fundamental structure is that it seems difficult to do in C. A quick search reveals possible implementations like the Judy Array or a simple linked list, but these approaches depend on dynamic allocation.

It seems like for our needs, a fixed-size array with a mask could still be efficient. For example, say we had a maximum of 20 axes, but only the first 3 active. The corresponding bitmask to this would be 1 + 2 + 4 = 7. We could take advantage of this when iterating over the joints with a clever for loop:

int index = 0;
for (int i = 1; i < mask; i =<< 1) {
    if (mask & i) {
    ...(do something with axis[index])...
    }
    index++;
}

This example would only have to run through 3 iterations, instead of the full 20. Of course, it wouldn't help you if your active joints were 17,18,19, but that seems like a rare case. While this is a bit uglier than a simply iterating over the whole array, you shouldn't have to do this much outside of the helper functions (i.e. copying, validity checks, arithmetic).

[ArcEye]

Comment by mhaberler Tue Apr 22 17:44:44 2014

---

sounds good. Nope, no we dont do dynamic allocation out here in RT land ;) Actually this permits an optimization when transmitting as protobuf - double array members following the last mask bit can be dropped, and the significant part held in a 'repeated double' (variable sized array really). There might be holes, but the only case I could think of is XZ (lathe).

(I need to eye speed and traffic a bit because remote preview might become a bottleneck)

the kernel has a grandiose and optimized bitmap library which would come handy - unfortunately GPL2 only: http://lxr.free-electrons.com/source/include/linux/bitmap.h#L25 , http://lxr.free-electrons.com/source/lib/bitmap.c

[ArcEye]

Comment by micges Tue Apr 22 18:18:49 2014

---

Side note: I worked with XY XYZ XYC XYZC XYUV XYZW kinematics

[ArcEye]

Comment by cdsteinkuehler Tue Apr 22 19:22:38 2014

---

On 4/22/2014 1:18 PM, Michael wrote:

Side note: I worked with XYC XYZC XYUV XYZW kinematics

There is no constraint that axis[0] = X and axis[1] = Y, is there?

...as long as both ends agree on the axis mapping, anything should be possible. Exactly what does "X" mean on my hypothetical 10-axis robot arm or 3-axis robot leg, anyway? IMHO, the fewer fundamental assumptions made by the core code, the easier it will be to expand beyond traditional CNC.

Charles Steinkuehler charles@steinkuehler.net

[ArcEye]

Comment by mhaberler Tue Apr 22 19:53:39 2014

---

sure, thats's just naming

where it would count is library calls which work on cartesians - those better know where x,y, and z are in that vector

[ArcEye]

Comment by mhaberler Wed Apr 23 06:44:02 2014

---

interesting find in context, while we're talking posemath: There is a library 'gomath' in https://github.com/machinekit/machinekit/tree/master/src/libnml/posemath which seems to be a cleaner rewrite of posemath. It seems to be used only in the genserkins.c code. I'll ask Alex Joni about it.

[ArcEye]

Comment by alexjoni Wed Apr 23 07:20:13 2014

---

genserkins is adapted from a work of Fred Proctor / Nist, and when I ported it to emc2/linuxcnc I found it easier to include gomath, rather than rewrite most of it to use posemath. I started initially, but some changes to posemath were necessary, and I wanted to eliminate touching that. AFAIK currently genserkins is the only place gomath is used.

[ArcEye]

Comment by mhaberler Wed Apr 23 17:00:57 2014

---

There is no constraint that axis[0] = X and axis[1] = Y, is there?

I think only ABC are for rotary axes.

[ArcEye]

Comment by mhaberler Wed Apr 23 17:20:27 2014

---

cursory impact analysis - looking at motion only for now, ignoring canon:

libraries referenced:

• emc/nml_intf/emcpose.o - very few and simple calls; references EmcPose
• src/libnml/posemath/_posemath.c, the 'realtime' version of src/libnml/posemath/posemath.cc - does NOT reference EmcPose - suggesting that beyond using code proper (below), _posemath.c would have to be adapted to work on a designed tuple of (x,y,z) in the new structure (correct?)

EmcPose usage in motion code proper:

grep -nH -e EmcPose command.c:79:extern void print_pose ( EmcPose _pos ); command.c:216:int inRange(EmcPose pos, int id, char move_type) command.c:364: EmcPose where; control.c:454:extern int inRange(EmcPose pos, int id, char move_type); // from command.c control.c:458: EmcPose nt = &emcmotStatus->pause_offset_carte_pos; control.c:459: EmcPose *ipp = &emcmotStatus->pause_carte_pos; control.c:623: EmcPose cpos; control.c:661: EmcPose cpos; control.c:674: EmcPose here; motiondebug.h:82: EmcPose oldPos; / last position, used for vel differencing _/ motiondebug.h:83: EmcPose oldVel, newVel; / velocities, used for acc differencing _/ motiondebug.h:84: EmcPose newAcc; / differenced acc / motion.h:81:#include "emcpos.h" / EmcPose / motion.h:105: EmcPose currentVel; motion.h:106: EmcPose currentAccel; motion.h:107: EmcPose desiredVel; motion.h:108: EmcPose desiredAccel; motion.h:218: EmcPose pos; / line/circle endpt, or teleop vector _/ motion.h:254: EmcPose tooloffset; / TLO _/ motion.h:636: EmcPose carte_poscmd; / commanded Cartesian position _/ motion.h:638: EmcPose carte_posfb; / actual Cartesian position _/ motion.h:640: EmcPose worldhome; / cartesean coords of home position / motion.h:653: EmcPose probedPos; / Axis positions stored as soon as possible motion.h:692: EmcPose dtg; motion.h:697: EmcPose tool_offset; motion.h:701: EmcPose pause_carte_pos; // initial pause point (ipp) - where we switched to the altQueue motion.h:702: EmcPose pause_offset_carte_pos; // ipp + current offset values, set by update_offset_pose()

kinematics: varies widely between kins, but trivkins: minimal:

trivkins.c:20: EmcPose * pos, trivkins.c:37:int kinematicsInverse(const EmcPose * pos, trivkins.c:56:int kinematicsHome(EmcPose * world,

tp: significant usage of EmcPose:

grep -nH -e EmcPose tc.c:233: * @return EmcPose returns a position (\ref EmcPose = datatype carrying XYZABC information tc.c:236:int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out) { tc.c:241:int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out) { tc.c:246:int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out) { tc.c:251:int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) tc.c:312: pmCartesianToEmcPose(&xyz, &abc, &uvw, pos); tc.c:536: EmcPose const * const start, tc.c:537: EmcPose const * const end) tc.c:562: EmcPose const * const start, tc.c:563: EmcPose const * const end, tc.c:634: EmcPose const * const start, tc.c:635: EmcPose const * const end) tc.h:25:int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); tc.h:26:int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); tc.h:27:int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); tc.h:28:int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); tc.h:64: EmcPose const * const start, tc.h:65: EmcPose const * const end); tc.h:70: EmcPose const * const start, tc.h:71: EmcPose const * const end, tc.h:77: EmcPose const * const start, tc.h:78: EmcPose const * const end); tp.c:531:int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos) tp.c:552:int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos) tp.c:568:int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp) tp.c:1273:int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxvel, tp.c:1647:int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double tp.c:1716: EmcPose end, tp.c:2163: EmcPose target; tp.c:2605: EmcPose before; tp.c:2636: EmcPose displacement; tp.c:2806: EmcPose before; tp.c:2813: EmcPose displacement; tp.c:2977: EmcPose pos_before = tp->currentPos; tp.c:2989: EmcPose disp; tp.c:3056:int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) tp.h:33:int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos); tp.h:34:int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); tp.h:35:int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); tp.h:36:int tpAddRigidTap(TP_STRUCT * tp, EmcPose end, double vel, double tp.h:38:int tpAddLine(TP_STRUCT * tp, EmcPose end, int type, double vel, double tp.h:40:int tpAddCircle(TP_STRUCT * tp, EmcPose end, PmCartesian center, tp.h:47:int tpGetPos(TP_STRUCT const * const tp, EmcPose \ const pos); tp_types.h:93: EmcPose currentPos; tp_types.h:94: EmcPose goalPos;

[ArcEye]

Comment by roggedaniel Thu Apr 24 13:38:41 2014

---

There is no constraint that axis[0] = X and axis[1] = Y, is there?

I believe that GUIs that use the emcmodule.cc interface assume status.position[0] is X and [1] is Y, etc.

Rogge

[ArcEye]

Comment by robEllenberg Thu Apr 24 22:12:36 2014

---

There is no constraint that axis[0] = X and axis[1] = Y, is there?

The array-based structure I mentioned doesn't have this limitation; I just chose those axes / indices as an example. That said, I'm sure that assumption is implicit or explicit in many places.

[ArcEye]

Comment by mhaberler Wed May 7 18:57:28 2014

---

I just learned about this package: https://pythonhosted.org/quantities/user/tutorial.html - it might be worth considering a unit-tagged notation for EmcPose and possibly other structures.

My nephew hinted me this is used at his company - they use such quantity-tagged values throughout their pipeline which prevents 'unit misunderstandings' like frequently happens with LinuxCNC and its 'Machine units' stored here and there and which can easily get out of sync.

(he just started at a chip mask exposure systems company, which is an elaborate form of motion control (ion beams) and they use - tataa - zeromq, protobuf and Qt as middleware and UI vehicle).

[ArcEye]

Comment by RunningLight Thu May 8 15:47:02 2014

---

On 05/07/2014 02:57 PM, Michael Haberler wrote:

I just learned about this package: https://pythonhosted.org/quantities/user/tutorial.html - it might be worth considering a unit-tagged notation for EmcPose and possibly other structures.

Be still, my beating heart. It's ridiculous in this day and age to still be carrying untagged physical quantities around, even more so as we contemplate loosely coupled systems. I won't bore you with tales of woe like NASA's loss of its US$125M Mars orbiter. I'll just second the motion. We who devised ISO 10303 STEP for the manufacturing industry built units of measure into its very core and that was decades ago.

Regards, Kent

[ArcEye]

Comment by mhaberler Wed May 20 06:57:56 2015

---

it turns out ROS is using SI throughout - the idea of using configurable units at the machine level as well as the UI level was a problem in the making to start with

that would be a sensible alternative to tagged quantities - it would mean all conversion happens in UI and interpreter from/to whatever is desired; from canon onwards it'd be all SI, including RT and feedback from there

[ArcEye]

Comment by cdsteinkuehler Wed May 20 13:53:10 2015

---

+1

I like the idea of just using SI units.

I design a lot of mixed-unit circuit boards (some parts "hard" metric, and some in inch units). Using SI units allows correct representation of inch with a bit of overhead (you need a few more decimal points), but representing metric in imperial units is a royal PITA and a recipe for many subtle bugs due to rounding issues (yes, I know from experience using poorly designed CAD tools that tried to layer metric support on top of an imperial unit core...gak!).

Charles Steinkuehler charles@steinkuehler.net

[ArcEye]

Comment by RunningLight Wed May 20 15:01:57 2015

---

As a purist, I have an affinity for tagged quantities, but as a pragmatist, I can live with strict SI conformance. Either way, we always know what we're dealing with when we receive data. I'll have to look at ROS in detail to see how they deal with powers of ten, e.g., the metre is the SI unit of length; the second, the SI unit of time. We're often, but not always, likely to want millimetres and microseconds. [Interesting - the spell checker on my tablet let me type metre but I had to override its autocorrection of millimetre to millimeter.]

[ArcEye]

Comment by RunningLight Wed May 20 16:08:38 2015

---

Ok, based on limited data (e.g., a 10 minute, er, 600 s, Google search), I conclude ROS does rigorously enforce SI, so units of measure in messages shall be metres/meters (m), seconds (s), newtons (N), radians (rad), Hertz (Hz), etc.

@cdsteinkuehler we're talking about information interchange here. It's still up to the writers of the sending and receiving software to get the conversion to/from SI right. Numerical issues will always be with us as long as non-SI units of measure continue to be useful to end users.