Advertising

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

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.

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-2000iB/165F,/210F with and without insulation plate.

R-2000iC/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.

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.

Files

Definitely some time involved in figuring this out I am sure. I have only learned a bit about matrix math; I don't have enough patience to learn more, and when I tried watching the web session it made me sleeeeeeepy. haha

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:

DH
parameters

Alpha (α)

a (mm)

d (mm)

Theta (θ) (deg)

Joint 1 DH
parameters

90

0

0

θ1

Joint 2 DH
parameters

0

260

0

90-θ2

Joint 3 DH
parameters

90

20

0

θ3-θ2+90

Joint 4 DH
parameters

-90

0

290

θ4*(-1)

Joint 5 DH
parameters

90

0

0

θ5

Joint 6 DH
parameters

0

0

0

θ6*(-1)

FacePlate DH
parameters

0

0

70

0

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.

Code

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

Display More

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.

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

How does that work? I'm not sure we're using the same terminology here -- I know that when I'm programming a Fanuc vs a KUKA, and using my hand to work out what angles I need to use, I have to rotate in completely different sequences.