# Pose\_trans and pose\_inv functions in URCap **URL:** \ **Category:** URCaps SDK\ **Created:** [October 13, 2017, 1:26pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257 "2017-10-13T13:26:15Z")\ **Posts on this page:** 12\ **Page:** 1
**Author:** ![anon51496239](https://avatars.discourse-cdn.com/v4/letter/a/94ad74/32.png) [@anon51496239](https://forum.universal-robots.com/u/anon51496239)\ **Post date:** [October 13, 2017, 1:26pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/1 "2017-10-13T13:26:15Z")
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 ---
**Author:** ![anon51496239](https://avatars.discourse-cdn.com/v4/letter/a/94ad74/32.png) [@anon51496239](https://forum.universal-robots.com/u/anon51496239)\ **Post date:** [October 17, 2017, 7:30pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/2 "2017-10-17T19:30:07Z")
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](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 \, 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. ---
**Author:** ![anon91082083](https://avatars.discourse-cdn.com/v4/letter/a/2bfe46/32.png) [@anon91082083](https://forum.universal-robots.com/u/anon91082083)\ **Post date:** [January 28, 2018, 5:49am UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/3 "2018-01-28T05:49:50Z")
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 ---
**Author:** ![chris.briedenhann](https://avatars.discourse-cdn.com/v4/letter/c/9de053/32.png) [@chris.briedenhann](https://forum.universal-robots.com/u/chris.briedenhann)\ **Post date:** [April 11, 2019, 8:28am UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/4 "2019-04-11T08:28:08Z")
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() [Maths - Matrix algebra - Inverse 4x4 - Martin Baker](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. [Maths - Conversion Matrix to Axis Angle - Martin Baker](http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm) Thanks Chris ---
**Author:** ![dmartin](https://avatars.discourse-cdn.com/v4/letter/d/c4cdca/32.png) [@dmartin](https://forum.universal-robots.com/u/dmartin)\ **Post date:** [July 9, 2019, 10:31am UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/5 "2019-07-09T10:31:33Z")
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? ---
**Author:** ![anon51496239](https://avatars.discourse-cdn.com/v4/letter/a/94ad74/32.png) [@anon51496239](https://forum.universal-robots.com/u/anon51496239)\ **Post date:** [July 10, 2019, 4:06pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/6 "2019-07-10T16:06:17Z")
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 > ---
**Author:** ![dmartin](https://avatars.discourse-cdn.com/v4/letter/d/c4cdca/32.png) [@dmartin](https://forum.universal-robots.com/u/dmartin)\ **Post date:** [July 16, 2019, 8:35am UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/7 "2019-07-16T08:35:37Z")
**Thank you very much @anon51496239!!!** 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 @anon51496239!! ---
**Author:** ![anon51496239](https://avatars.discourse-cdn.com/v4/letter/a/94ad74/32.png) [@anon51496239](https://forum.universal-robots.com/u/anon51496239)\ **Post date:** [July 18, 2019, 4:04pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/8 "2019-07-18T16:04:44Z")
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});` 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);` 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 ---
**Author:** ![anon65613222](https://avatars.discourse-cdn.com/v4/letter/a/3ec8ea/32.png) [@anon65613222](https://forum.universal-robots.com/u/anon65613222)\ **Post date:** [October 16, 2019, 1:51pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/9 "2019-10-16T13:51:32Z")
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! > **[Rasmus L / RobotKinematics · GitLab](https://gitlab.com/Tirine/robotkinematics)** > > Robot kinematics and useful functions for UR. Additions to the work by: paul weidinger, pweids, https://gist.github.com/pweids Find his work in the UR Forum:... 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. ---
**Author:** ![marius.gheorghe](https://avatars.discourse-cdn.com/v4/letter/m/bbce88/32.png) [@marius.gheorghe](https://forum.universal-robots.com/u/marius.gheorghe)\ **Post date:** [February 11, 2021, 3:29pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/10 "2021-02-11T15:29:09Z")
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! ---
**Author:** ![Ebbe](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/ebbe/32/1413_2.png) [@Ebbe](https://forum.universal-robots.com/u/Ebbe)\ **Post date:** [February 12, 2021, 2:47pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/11 "2021-02-12T14:47:12Z")
You can also use [this](https://forum.universal-robots.com/t/urcap-sample-urcap-scriptcommunicator/8460) project to get the same result as URControl ---
**Author:** ![system](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/system/32/1_2.png) [@system](https://forum.universal-robots.com/u/system)\ **Post date:** [February 14, 2021, 2:47pm UTC](https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/12 "2021-02-14T14:47:56Z")
Closed on February 14, 2021, 2:47pm UTC This topic was automatically closed 2 days after the last reply. New replies are no longer allowed.