Non-trivial kinematics for Fanuc S420F Arm, 6dof with 4-bar linkage

From what I can find around this forum and others, I'm going to have to learn to write/adapt a kinematics module for this, but if anyone can suggest methods to work around it please do. I'm working on a Fanuc S420F robot arm, driving the motors with Stmbl drives, which are starting to behave themselves, and will need to work on integrating it with linuxcnc fairly soon once all motors are tuned and mounted.

Ages ago I got the DH parameters figured out, altered files for pumakins and vismach to suit, and got a working virtual rig whose motion made sense for a serial arm with the same lengths and angles as the Fanuc, so probably the parameters are correct. However, I've since managed to get the arm moving to some extent and it looks like the kinematics are not straightforwardly serial.

There is a 4-bar parallelogram linkage mechanism between joints 2 and 3 (calling the base = j1 and end effector = j6) meaning that when j2 angle changes, j3 angle also changes (without j3 motor moving) such that when the shoulder joint moves, the arm maintains its angle relative to the base, rather than relative to j2. From forum posts elsewhere (www.robot-forum.com/robotforum/thread/22...%2Blinkage#post97306 and www.robot-forum.com/robotforum/thread/13...%2Blinkage#post54255) it seems that the original controller compensates for this, such that if you jog j2 it will drive both j2 and j3 to keep the angle between them static.

So...where might be the best place to implement this? Is it possible to rig the hal file with some logic that drives j3 on a j2 jog but not vice versa, and that only in world mode but not joint mode, or something? Or is this a roll-your-own kinematics module as I suspect/fear it might be?

P.S., recently a couple of guys (I think they are FreaKontrol in Italy) have updated the stmbl design addressing a number of problems including the obsolete power stage, and are working on making these available, which could breathe some very welcome new life into the stmbl project. Very exciting to see.

kind regards to all,
Andy