Position Converter - Convert from XYZWPR Rep to Joint Rep and Back

Now Open Source:

https://github.com/SynapticRobotics/PositionConverter

Intro:

I've created a freeware tool I would like to share with the community. I've named it Position Converter. Position Converter’s purpose is to convert Fanuc TP program points from XYZWPR representation to Joint representation and vice versa. The program does this by opening a full MD: backup and parsing all .ls files contained along with a number of various files for the robot’s arm type, user tools, and user frames in order to do the conversion properly.

What Position Converter looks like when a backup is opened:

Installation:

Download and extract the attached .zip file into a folder somewhere.
Make sure your computer has .Net 4.6.1 Framework installed.

Program Tips:

• Hovering over a point will display its contents in a tooltip.
• Hovering over a program will display its comment in a tooltip.
• Points are color coded by their point representation. Green for XYZWPR, and blue for Joint.
• If a conversion fails, the point will be colored red, and the tooltip will be updated on why the conversion failed.
• You can view the parsed in robot type, user frames and robot model under the view tab.

Known Issues:

• The math will have slight differences from a conversion done on the Fanuc controller vs a conversion done by this program. Typically the differences are in the order of hundredths to thousandths of a millimeter (far below the repeatability of a typical arm).
• Only NUT or FUT point configurations are supported. I plan on adding support for NDT, FDT, NDB, and FDB at a later date.
• Only the following arms are supported, more to be added in the future:
  • R-1000iA/80F with and without insulation plate.
  • R-1000iA/100F with and without insulation plate.
  • R-2000iA/165F,/200F with and without insulation plate.
  • R-2000iB/125L,/165F,/210F with and without insulation plate.
  • R-2000iC/125L,/165F,/210F with and without insulation plate.
  • Arc Mate 120iC or M20iA
  • M710iC/12L
  • M710iC/20L - Untested
  • M710iC/20M - Untested
  • M710iC/45M - Untested
  • M710iC/50 - Untested
  • M710iC/70 - Untested
• Multiple groups are now kinda supported, but only the first group will get converted.
• Remote TCP is not supported.
• Extended Axes are not supported.

Help Me Out:

This release is pretty much alpha level. As such there will be bugs and crashes. Please let me know if this program works out for you, or if you would like to have a particular arm added. If the program craps out for any reason, please dwagner@synapticrobotics.com the backup so I can fix the issue. I plan on supporting this for a while.

Revision History:

V1.1.1.0 - Initial Release

V1.1.1.1 - Added R2000iA/165 and 200F variants

V1.1.1.2 - Added R2000iB/125L and R2000iC/125L variants

V1.1.1.3 - Added .csv output for programs when 'saving as'. Outputs a .csv of the program points.

Quote from bagged2drag

You can do this in Karel as well and deploy in roboguide or on an actual controller. It is nice for some applications. Good job!

True, but all those cost money.

Several years ago, I wrote a routine in Karel that does the same thing as this (attached), but the IK solving is a black box. I mainly used this program to as an exercise in order to learn how to solve inverse kinematics via matrix math, and to get a better understanding of matrix math in general. Turns out solving a robot with a spherical wrist is not too hard once you have your head around the math behind it.

Quote from Strini

The tool needs a 'version.dg' file that is missing in my backups.

Did you take a full MD: backup?

Yes, the robot's kinematics are modeled in the program. All forward and inverse kinematics are done in it.

I mainly wrote it for a customer of mine that needs to do this task frequently. They have a path in car body space coordinates, robot gets installed in the plant, the service tech touches up the path to match reality, then the laser tracker guy comes in to shoot in the robots into body space, but the tech doesn't want to lose his touch ups, so he converts his whole path to joint. Once the real body space transform is known, they convert the path back to XYZWPR.

If they are lucky, they get the laser tracker guy in before they do the touch up, but the laser tracker guy is usually slammed.

I have the DH parameters hard coded into the tool for now. To figure out which DH parameters to use, the tool just parses the version.dg file and then looks up the arm in a look up table. I should break the arm data out into their own file structure. DH parameters are derived from the data sheet on Fanuc's website. If I wanted to get fancy, I could pull the DH parameters out of the system variables, which are stored in $PARAM_GROUP[1].$DH_A and $PARAM_GROUP[1].$DH_D last time I checked.

For that arm it looks like the DH parameters are:

Note that since Fanucs place their origin at the intersection of J1 and J2, you can ignore the 330mm from the base to J2.

You could combine the J6 and Faceplate DH parameters, but I broke them out because it made it easier for me.

Regarding the DH parameters you found, they are pretty much the same, it is just that they do a -90 in alpha on J3 whereas I do a +90. They then "undo" this at J6 with the 180 alpha. By defining the J3 at a -90, they have to define all the folowing d's negative, and take opposite rotations on alpha to J6. I see why they did this though. By doing it that way, they don't have to negate their J4 and J6 theta's like I do.

I use (and apparently so does Fanuc) the equations defined in the Wikipedia article about DH parameters. This is what your first two papers use.

Function DH_TransformMatrix(alpha As Range, a As Range, d As Range, theta As Range)

    Dim Mr(4, 4) As Double  ' The final result matrix
    'Below is the transform matrix from the article on wikipedia about DH parameters.
    Mr(0, 0) = Cos(theta)
    Mr(0, 1) = -Sin(theta) * Cos(alpha)
    Mr(0, 2) = Sin(theta) * Sin(alpha)
    Mr(0, 3) = a * Cos(theta)
    Mr(1, 0) = Sin(theta)
    Mr(1, 1) = Cos(theta) * Cos(alpha)
    Mr(1, 2) = -Cos(theta) * Sin(alpha)
    Mr(1, 3) = a * Sin(theta)
    Mr(2, 0) = 0
    Mr(2, 1) = Sin(alpha)
    Mr(2, 2) = Cos(alpha)
    Mr(2, 3) = d
    Mr(3, 0) = 0
    Mr(3, 1) = 0
    Mr(3, 2) = 0
    Mr(3, 3) = 1
    DH_TransformMatrix = Mr

End Function

The third paper is using the modifed DH parameters, which are sometimes refered to as just DH parameters. Which tends to lead to a bit of confusion.

I've been testing both sets of DH parameters and am getting good results. Hopefully we can find out where your error is coming in.

I'm not familiar with that nomenclature. I would expect a 4x4 matrix, or XYZWPR representation.

It won't be aligned. The numbers you are getting are the same as a user tool on the face plate. Z will be lined up with world X, and X with world Z.

You should be getting the following:

Since most Fanuc arms are spherical wrists, the inverse kinematics become a bit simpler. Look up kinematic decoupling if you would like to read more.

The thousand foot view of how it works is that you use the wrist center point to solve for the major axes, and then you use the WPR of the point to fine the minor axes.

How my conversion utility works:

1. Convert point to matrix representation. Fanucs are world Rz*Ry*Rx, so you can use that to get back to matrix rep.
2. Take the point and matrix multiply it by the inverse of the TCP to get back to the Faceplate.
3. Then times that by the inverse of the wrist center point to the faceplate to get back to the wrist center.
4. Once the wrist center point is found, check for reachability, and if reachable, solve J1, J2, and J3.
5. Now solve for J4, J5, and J6.

Yes they are. However, we are both right! Rx - Ry' - Rz'' is equivalent to Rz - Ry - Rx.