Pose_trans and pose_inv functions in URCap

URCap Development URCaps SDK
1

I need to access the results of the pose_trans() and pose_inv() functions in my URCap. I first tried using the Realtime client to send the URScript commands to Polyscope which then sends the result to my daemon, but it seems the URCap -> RTC -> Polyscope -> Daemon communication pipeline does not work.

So does anyone have a java-based implementation of pose_trans and pose_inv before I dive deep into my linear algebra?

Thanks

2

For future readers, I managed to re-implement pose_trans and pose_inv in java.

You first have to convert the pose to an affine transformation. To do this, you need to calculate the rotation vector as a 3x3 rotation matrix (see Maths - AxisAngle to Matrix - Martin Baker).

Then you extend that 3x3 rotation matrix to the 4x4 affine transformation by adding a 4th column vector that corresponds to <x,y,z,1>, and a 4th row vector <0,0,0,1> so it looks like

|   R   | T | 
|-------+---| 
| 0 0 0 | 1 |

where R is your rotation vector and T is the translation (XYZ) vector.

pose_inv() is as simple as taking the inverse of that matrix, whose formula is like so:

|   R   | T |    |  R^-1 | -(R^-1)*T |
|-------+---| -> |-------+-----------|
| 0 0 0 | 1 |    | 0 0 0 |     1     |

And pose_trans(p1, p2) is as simple as multiplying the two affine matrices, A and B, corresponding to poses p1 and p2 respectively:

|   R1  | T1 |    |   R2  | T2 |
|-------+----| X  |-------+----|
| 0 0 0 |  1 |    | 0 0 0 |  1 |

For my implementation, I had a class to handle matrix manipulations, a class to handle converting to/from rotation vector and transformation matrix, and a class to represent the pose and associated functions.

Hope this helps.

4 Likes
3

I am looking to be able to calculate post_trans in java as well - do you happen to have any code examples that you are able to share?

Thanks

4

Good Morning Paul

Thank you for adding this to the forum, I am also attempting to do this.

Just a couple of questions:

Thanks

Chris

5

Hi everybody!

Is there anyone happy to share the code for this pose_trans and pose_inv functions in Java?

Hi UR team!
I think this is a very usefull tool to share, via API, or a spare class. Would that be possible?

6

Sure @dmartin. I wrote 3 classes to use for client-side matrix and pose transformation math in Java. Built it from the links above. Hopefully the comments are able to direct you further.

Matrix class for standard matrix manipulation

Pose class that reimplements pose_trans() and other functions

Axis-Angle class to help with converting and using rotation-vector style notation

4 Likes
7

Thank you very much @paul.weidinger!!!

I’ve being catching up as much as possible with all this matrix math that I had abandoned since university… but still can’t figure out how to put everything together.

I’m so sorry to ask again. But, just in case you have a moment, I’d be very grateful if you can guide me through a little more…

I summary, I have a pose including TCP (X, Y, Z, Rx, Ry, Rz), I have that TPC (X, Y, Z, Rx, Ry, Rz), and I need to calculate the pose without TCP.

  • How can I convert the pose including TCP from (X, Y, Z, Rx, Ry, Rz) [m, rad] format to a vector to be transformed? Should this be put in form of a 3 fields vector? How?
  • How can I get the transformation matrix for a given TCP (X, Y, Z, Rx, Ry, Rz) [m, rad] ? To ‘take out’ the TPC, it should be inverted, right?
  • How do I get the final result and put it back to (X, Y, Z, Rx, Ry, Rz) [m, rad] format?

Thank you again @paul.weidinger!!

8

I don’t have a robot around right now, otherwise I’d be able to do a little trial and error to make sure this is correct. I encourage you to do so yourself (or somebody else to chime in).

  1. Converting TCP to a Pose class. Use one of the constructors or the setters to create your Pose class.
    Example:
    double X = ...;
    double Y = ...;
    double Z = ...;
    double Rx = ...;
    double Ry = ...;
    double Rz = ...;
    Pose pose = new Pose(new double[] {X, Y, Z, Rx, Ry, Rz});

    If you want the vector, you can do pose.getRotationVector();

  2. To get the transofrmation matrix, you can do pose.getTransformationMatrix(); But it’d be easier to just do pose.invert(); To back out the TCP, yes I believe you invert it and apply it. The easier thing would be to use pose_trans. Play with these two functions to see if it works for you (i.e. compare it to UR’s calculated value)
    Either 1:
    Pose tcp = new Pose(...);
    Pose poseNoTCP = pose.pose_trans(tcp);

    or 2:
    Pose tcp = new Pose(...);
    Pose poseNoTCP = tcp.pose_trans(pose)

  3. The final result (poseNoTCP) is already in (X, Y, Z, Rx, Ry, Rz) format.

Again, I recommend just playing around with the classes (try something like repl.it) and see what matches what the UR teach pendant shows. There are enough helper functions in the Pose class that I doubt you’ll need to work with matrices or axis-angle representations directly.

Paul

1 Like
9

I really like your work, but I found it while I was looking for Inverse and Forward Kinematics, so I added some on your work!

Mainly Forward Kin in the RobotJoints.java

I’m still working on the Inverse Kin, in the RobotPose.java, it doesn’t always work. I needed it quite quickly, so I just threw something together. As the first 3 joints change the position mostly, I only use those for coordinates change and only the last 3 joints for rotation.
As you can see there, I also did an attempt on Jacobian Inverse, and it wasn’t working very well, so I just scrapped it for now and maybe I’ll go back to it later.

2 Likes
10

Hey,
I actually had some tests with your code and I found some cases with undefined results. The issue is that in the gaussianDeterminant(double[][] matrix) function there should be a check for matrix[i][i] == 0. Else there will be some divisions by 0 that will later end up in numbers with undefined values.

Thank you!

11

You can also use this project to get the same result as URControl

12

This topic was automatically closed 2 days after the last reply. New replies are no longer allowed.