This section is a summary about how to set up the kinematics with working tool length compensation. In the following picture, you can see this type of machine and all used joints, coordinate systems and points mentioned in the source code of the kinematics module.
In the picture the arrow of the joints indicates the direction where the number on the ruler on the machine increases. So if you command to drive in positive direction, the machine moves in reality in opposite direction of the arrow for joint 1, 2 and 3. Note that in the source code the joint numbers start from zero!
You get the equations which describe the kinematics by
* Going from the machine's origin to the "machine's point" via a = the blue long vector b = minus the green vector, -minus the red vector, orange vector, (black vector and purple vector) Vector a) and b) must be equal -> this gives three equations * joint 4 and A are equalNote: For kinematics equation calculation I ommited the tool offset in the first place and just used the vector from the absolute g-code coordinate system's origin to the "machine point". Later I substituted it with: back vector plus purple vector. I think this makes solving the equations easier and the source code more understandable. Second Note: The reason why the absolute g-code coordinate system is shifted by the height of the rotatory axis in negative z-direction from the coordinate system on the rotator axis is that I wanted that the value of the Z-axis is the same as the value of joint 3 if the value of the A-axis is zero (nearly trivkins behaviour).
The basic configuration of this machine was generated by stepconf by selecting the XYZA template. It has to be extended to XYZA(BC)UVW. B and C are not used. U, V, and W describe an offset in the tool's local coordinate system. X, Y and Z describe the tool tip's position in the absolute g-code coordinate system. A is the rotation of the tool tip around the x axis. This components and the joints are passed in and out of the kinematics module.
Note: The absolute W coordinate is stored in a joint in the inverse kinematic to have it at disposal in the forward kinematic. When the user changes the tool offset - with G43 H for example - then the interpreter changes the tool offset. Since the absolute W coordinate cannot jump - because it comes directly from a joint, and a joint cannot jump (except when homing) - the jump is made in the relative W coordinate. Normally one wants the absolute W coordinate to be the tool length. So a solution is to program G0 G53 W<tool length> to drive the absolute W coordinate to the tool length, then the relative W coordinate will be zero (if no other offsets are in place) - then one can also apply relative tool movements via U, V and W in the g-code program. Tool lengths are stored in the tool table in the W field.
Additional changes in the ini config file:
* you have to add the additional axes * you have to limit the max. rotational speed of the rotatory axis, because it seems that the trajectory generator ignores the max. speeds from the AXIS definitions (I think they are currently axes limits - in my opinion it would make more sense to define maximum speeds on joints). If for example A is 0 and one programs G0 A180, then not only joint 4 will rotate, but also joint 2 and 3 will move. If you do not limit the speed of the rotatory axis you will get "following errors" on joint 2 and 3. A suitable worest case estimation for me was: max speed of joint 4 * maximum tool distance from rotatory axis = max speed of joint 2 and 3. G-code programs work fine this way, also jogging in joint mode. In teleop mode I still often get following errors.
Additional changes in the hal file
* you have to compile the rot4thaxiskins with comp. * you have to load the this new kinematics modules in the hal file instead of the old one. loadrt rot4thaxiskins * you have to set the height of the rotatory axis with respect to the machine origin as a hal input to the kinematics module setp rot4thaxiskins.H1 41 * you have to connect the feedback pins of the new joints - otherwise you get following errros net upos-fb axis.6.motor-pos-cmd => axis.6.motor-pos-fb net vpos-fb axis.7.motor-pos-cmd => axis.7.motor-pos-fb net wpos-fb axis.8.motor-pos-cmd => axis.8.motor-pos-fb