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 javabased implementation of pose_trans
and pose_inv
before I dive deep into my linear algebra?
Thanks
For future readers, I managed to reimplement 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 http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm).
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.
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
Good Morning Paul
Thank you for adding this to the forum, I am also attempting to do this.
Just a couple of questions:
 Do I have to set up a matrix for the RX, RY and RZ rotations.
 Do I use the radians values for the calculations.
 Can I use the formulas in the link below for the pose_inv()
http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm  Do I use the formulas in the link below to get back to my pose.
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
Thanks
Chris
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?
Sure @dmartin. I wrote 3 classes to use for clientside 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
AxisAngle class to help with converting and using rotationvector style notation
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!!
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).

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();

To get the transofrmation matrix, you can do
pose.getTransformationMatrix();
But it’d be easier to just dopose.invert();
To back out the TCP, yes I believe you invert it and apply it. The easier thing would be to usepose_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)

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 axisangle representations directly.
Paul
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.
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!
This topic was automatically closed 2 days after the last reply. New replies are no longer allowed.