# Calculate plane from 3 points **URL:** \ **Category:** URScript\ **Created:** [June 19, 2018, 11:54am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142 "2018-06-19T11:54:07Z")\ **Posts on this page:** 14\ **Page:** 1
**Author:** ![anon67860743](https://avatars.discourse-cdn.com/v4/letter/a/13edae/32.png) [@anon67860743](https://forum.universal-robots.com/u/anon67860743)\ **Post date:** [June 19, 2018, 11:54am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/1 "2018-06-19T11:54:07Z")
Hi! I need to calculate a “plane pose” from 3 points the same way as a feature plane is calculated. Though I cant really figure out how to do that with the available urmath functions. Does anyone know how to do this? The feature plane is defined in this way (cut and paste from the manual): The position of the coordinate system is the same as the position for the first sub point. The z-axis is the plane normal, and the y-axis is directed from the first point towards the second. The positive direction of the z-axis is set so that the angle between the z-axis of the plane and the z-axis of the first point is less than 180 degrees. ---
**Author:** ![elvintoh82](https://avatars.discourse-cdn.com/v4/letter/e/bbce88/32.png) [@elvintoh82](https://forum.universal-robots.com/u/elvintoh82)\ **Post date:** [June 20, 2018, 4:39am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/2 "2018-06-20T04:39:23Z")
here u can use mine. ``` def global_plane_finder(p1, p2, p3): ``` V12 = [p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]] V13 = [p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]] VZ = mycross(V12, V13) Tmat = pose2Tmat(p1) VZofp1 = [Tmat[2], Tmat[6], Tmat[10]] value = mydot(VZ, VZofp1) if (value \< -0.00001): VZ = mycross(V13, V12) end VZ = myunitised(VZ) VY = myunitised(V12) VX = mycross(VY,VZ) output = [VX[0], VY[0], VZ[0], p1[0], VX[1], VY[1], VZ[1], p1[1], VX[2], VY[2], VZ[2], p1[2], 0, 0, 0, 1] pose = Tmat2pose(output) return pose end def mycross(a, b): output = [0, 0, 0] output[0] = a[1]\*b[2] - a[2]\*b[1] output[1] = a[2]\*b[0] - a[0]\*b[2] output[2] = a[0]\*b[1] - a[1]\*b[0] return output end def Tmat2pose(Tmat): output = p[0, 0, 0, 0, 0, 0] output[0] = Tmat[3] output[1] = Tmat[7] output[2] = Tmat[11] sy = mynorm([Tmat[0], Tmat[4], 0]) if (sy \> 0.00001): x = atan2(Tmat[9], Tmat[10]) y = atan2(-Tmat[8], sy) z = atan2(Tmat[4], Tmat[0]) else: x = atan2(-Tmat[6], Tmat[5]) y = atan2(-Tmat[8], sy) z = 0 end rotvec = rpy2rotvec([x, y, z]) output[3] = rotvec[0] output[4] = rotvec[1] output[5] = rotvec[2] return output end def pose2Tmat(pose): if (pose[3]) == 0 and (pose[4] == 0) and (pose[5] == (0)): Rmat = [1, 0, 0, 0, 1, 0, 0, 0, 1] else: x = pose[3] y = pose[4] z = pose[5] ang = mynorm([x, y, z]) x = x/ang y = y/ang z = z/ang s = sin(ang) c = cos(ang) t = 1 - c Rmat = [c+(t_x_x), (t_x_y)-(s_z), (t_x_z)+(s_y), (t_x_y)+(s_z), c+(t_y_y), (t_y_z)-(s_x), (t_x_z)-(s_y), (t_y_z)+(s_x), c+(t_z_z)] end Tmat = [Rmat[0], Rmat[1], Rmat[2], pose[0], Rmat[3], Rmat[4], Rmat[5], pose[1], Rmat[6], Rmat[7], Rmat[8], pose[2]] return Tmat end def mynorm(a): base = sqrt(a[0]\*a[0] + a[1]\*a[1] + a[2]\*a[2]) return base end def myunitised(a): output = [0, 0, 0] base = mynorm(a) output[0] = a[0] / base output[1] = a[1] / base output[2] = a[2] / base return output end ---
**Author:** ![anon67860743](https://avatars.discourse-cdn.com/v4/letter/a/13edae/32.png) [@anon67860743](https://forum.universal-robots.com/u/anon67860743)\ **Post date:** [June 20, 2018, 7:50am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/3 "2018-06-20T07:50:33Z")
Thanks man, really appreciate it. I will look through the code, looks very similar to what I had in mind if it wasnt solvable with the built in functions. ---
**Author:** ![mbush](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/mbush/32/76_2.png) [@mbush](https://forum.universal-robots.com/u/mbush)\ **Post date:** [July 23, 2018, 9:08pm UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/4 "2018-07-23T21:08:43Z")
@elvintoh82 I tried running this but the function mydot is not defined in the code. Can you please post that function? Thanks, Matt ---
**Author:** ![ajp](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/ajp/32/471_2.png) [@ajp](https://forum.universal-robots.com/u/ajp)\ **Post date:** [July 24, 2018, 1:10am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/5 "2018-07-24T01:10:14Z")
The way features are calculated from the points has changed from CB3 software v3.6 onwards and all e-Series software. If you use this method on those software versions you’ll get a different resulting plane than if you use the feature wizard. Not necessarily a problem but something to be aware of. [https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-36xx/](https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-36xx/) ---
**Author:** ![mbush](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/mbush/32/76_2.png) [@mbush](https://forum.universal-robots.com/u/mbush)\ **Post date:** [July 24, 2018, 4:56am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/6 "2018-07-24T04:56:26Z")
It would be great if UR would release this ability in URScript so that it could natively solve this problem. This request has been brought up by many individuals as a way to solve for moving robots from machine to machine. ---
**Author:** ![elvintoh82](https://avatars.discourse-cdn.com/v4/letter/e/bbce88/32.png) [@elvintoh82](https://forum.universal-robots.com/u/elvintoh82)\ **Post date:** [July 24, 2018, 7:50am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/7 "2018-07-24T07:50:39Z")
Here you go, it is basically just a simple dot product. def mydot(a, b): output = a[0]\*b[0] + a[1]\*b[1] + a[2]\*b[2] return output end ---
**Author:** ![anon87996651](https://avatars.discourse-cdn.com/v4/letter/a/46a35a/32.png) [@anon87996651](https://forum.universal-robots.com/u/anon87996651)\ **Post date:** [March 15, 2019, 7:37pm UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/8 "2019-03-15T19:37:53Z")
@ajp Has UR released a way to do this with script code yet? @elvintoh82 Firstly, thank you so much for putting this together. Second, you wouldn’t by chance have an updated version of this for CB3 series software v3.6 onwards and eSeries? I want to make use of UR’s capability to program waypoints relative to a feature, and then update that plane variable with this calculation from 3 “touchoff” points that the operator would teach. adding @mbush as he might want to see responses. ---
**Author:** ![ajp](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/ajp/32/471_2.png) [@ajp](https://forum.universal-robots.com/u/ajp)\ **Post date:** [March 18, 2019, 9:44am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/9 "2019-03-18T09:44:32Z")
Hi @anon87996651, nothing officially integrated into the software yet, but one of my UR colleagues recently put together some documentation explaining various aspects of how the UR system handles coordinate transforms etc. It includes some example script functions one of which is get\_feature\_plane(), which do what you need for the post v3.6 planes. > [@Robot theory and how to use it in UR robots](https://forum.universal-robots.com/t/robot-theory-to-control-ur-robots/3863): > > This is the theoretical explanation on general robot manipulators and how to use it for the advanced applications with UR robots. Attached the URScript sample is written for the purpose of better understanding. However, it has not been tested so that it is possible to have errors. Looking forward to your awesome development with this! [Robotics in UR Robots\_March 2019.pdf](https://us1.discourse-cdn.com/flex020/uploads/universal_robots/original/2X/d/d4588fd696b452e4b6566f4f44ada81d9f6e6fe6.pdf) (1.5 MB) [robotics.zip](https://us1.discourse-cdn.com/flex020/uploads/universal_robots/original/2X/a/ace3805217576f15b30c4f93253de3b0af5473b1.zip) (3.1 KB) ---
**Author:** ![anon87996651](https://avatars.discourse-cdn.com/v4/letter/a/46a35a/32.png) [@anon87996651](https://forum.universal-robots.com/u/anon87996651)\ **Post date:** [March 19, 2019, 5:39pm UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/10 "2019-03-19T17:39:07Z")
@ajp this is perfect, very informative, easy to follow, and has the code examples for the exercises I tested it and there were only a few syntax errors in the .script file. For instance, step 7 of the get\_feature\_plane() function should have the assignment be to a pose variable but the ‘p’ is missing in front of the brackets. Once those were corrected, everything works great! ---
**Author:** ![ajp](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/ajp/32/471_2.png) [@ajp](https://forum.universal-robots.com/u/ajp)\ **Post date:** [March 21, 2019, 4:08am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/11 "2019-03-21T04:08:22Z")
great! If you found any other syntax issues please let us know. (@sba ) ---
**Author:** ![mkg](https://avatars.discourse-cdn.com/v4/letter/m/ea666f/32.png) [@mkg](https://forum.universal-robots.com/u/mkg)\ **Post date:** [June 30, 2019, 8:17am UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/12 "2019-06-30T08:17:51Z")
Hi jts, What is the code for Rotmat2rotvec function? ---
**Author:** ![anon87996651](https://avatars.discourse-cdn.com/v4/letter/a/46a35a/32.png) [@anon87996651](https://forum.universal-robots.com/u/anon87996651)\ **Post date:** [July 8, 2019, 11:56pm UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/13 "2019-07-08T23:56:41Z")
Hi @mkg , I got the script code from the .zip file from @ajp 's response from earlier in this thread. Here is the link to the thread : > [@Robot theory and how to use it in UR robots](https://forum.universal-robots.com/t/robot-theory-and-how-to-use-it-in-ur-robots/3863): > > This is the theoretical explanation on general robot manipulators and how to use it for the advanced applications with UR robots. Attached the URScript sample is written for the purpose of better understanding. However, it has not been tested so that it is possible to have errors. Looking forward to your awesome development with this! [Robotics in UR Robots\_March 2019.pdf](https://us1.discourse-cdn.com/flex020/uploads/universal_robots/original/2X/d/d4588fd696b452e4b6566f4f44ada81d9f6e6fe6.pdf) (1.5 MB) [robotics.zip](https://us1.discourse-cdn.com/flex020/uploads/universal_robots/original/2X/a/ace3805217576f15b30c4f93253de3b0af5473b1.zip) (3.1 KB) The robotics.zip file has a .script file with the code in it. ---
**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:** [January 21, 2021, 1:35pm UTC](https://forum.universal-robots.com/t/calculate-plane-from-3-points/2142/14 "2021-01-21T13:35:02Z")
This topic was automatically closed 2 days after the last reply. New replies are no longer allowed.