June 18, 2019, 01:09:09 PM

# ABC 3 point BASE measurement. How does algorithm works?

### Author Topic:  ABC 3 point BASE measurement. How does algorithm works?  (Read 17273 times)

0 Members and 1 Guest are viewing this topic.

December 25, 2014, 08:01:26 AM

#### ShaM

##### ABC 3 point BASE measurement. How does algorithm works?
I want to perform ABC 3 point BASE measurement programmatically. Let's suppose I know the 3 point of the model and I can move the robot to it. Is it possible to call the measurement method in robot program?

Today at 01:09:09 PM

Guest
##### Re:  ABC 3 point BASE measurement. How does algorithm works?

December 25, 2014, 06:36:48 PM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
not that I know of... you will need to make own function and if that is what you want, you will need to dive into math and KRL programming.

for example, teaching base with 3-points means that points are:
P1 - origin
P2 - point on X axis
P3 - point on XY plane (positive Y)
from each of the three points you only need translation components X,Y,Z, orientation A,B,C and Status and Turn do not matter...

to create new base you just need to assign frame data {X,Y,Z,A,B,C} to base of your choice.
XYZ components are easy - they are already in P1 (if P1 is expressed in WORLD)
however, determining ABC is a bit more work and requires knowing how the rotations are computed on kuka, then solving inverse of a rotation matrix.
and this will take some math skill...

order of rotations matter. in kuka, rotations are done in order A > B> C where

see
http://en.wikipedia.org/wiki/Euler_angles

suppose you start with WORLD coordinate system where unit vectors that form base are <i,j,k>
after transform through rotation matrix you get <i',j',k'>

we know that
i=(1,0,0)
j=(0,1,0)
k=(0,0,1)

you can compute i',j',k' from your points and the rotation matrix. (forgive me for any typos, i am still in party mode, got to love eggnog  )

for example i' is a unit vector in same direction as a vector P12 which is from P1 to P2.
in fact all we need to do is normalize P12 and that is our i'

to get k' we need to compute cross product P12xP13 and normalize it.

to get j' we just do a cross product of k' x i'
btw normalizing vector means dividing each of its component by vector length
For example:

P12=(P2.x-P1.x, P2.y-P1.y, P2.z-P1.z)
|P12| = sqrt( (P12.x)^2 + (P12.y)^2 + (P12.z)^2)
i'=(1/|P12|)*P12

P13=(P3.x-P1.x, P3.y-P1.y, P3.z-P1.z)
|P13| = sqrt( (P13.x)^2 + (P13.y)^2 + (P13.z)^2)
u'=(1/|P13|)*P13

k'= i' x u'
j' = k' x i'

now that the easy part is done (i', j', k' are columns of rotation matrix), you still need to determine values of angles A,B, C that result in that particular rotation matrix:
« Last Edit: December 25, 2014, 07:03:23 PM by panic mode »

December 25, 2014, 06:50:36 PM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
C and S are just short hand notation for COS and SIN functions. since rotations are A,B,C (or about axes Z>Y>X), that should be:
C1=Cos(A)
C2=Cos(B)
C3=Cos(C)
S1=Sin(A)
S2=Sin(B)
S3=Sin(C)

December 26, 2014, 06:12:07 AM

#### kaneyja

##### Re: ABC 3 point BASE measurement. How does algorithm works?
hello panic mode !
i really really need your knowledge.
but i can't understand ....

for example...
i have 3 points like below
Origin   194.380200   1665.665000   1041.952000
x axis    201.715089   1561.051559   1086.998435
y axis   294.122864   1671.903273   1041.523605

how can i calculate A, B, C?

i want to set base coordinate
BASE_DATA[1] = {x 0.0, y 0.0, z 0.0, A 0, B 0, C 0}

x, y, z is 194.380200, 1665.665000, 1041.952000

but i can't calculate A , B, C...

December 26, 2014, 06:52:23 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
My respect to you, panic mode!
Your explanation is so good for me, I got it, only the one thing to make clearer.
Is the rotation matrix looks like this?

December 26, 2014, 07:01:40 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
kaneyja, panic mode just explained all at the up. You need to calculate rotation matrix and from it find ABC values. It looks pretty clear, read it one more time

December 26, 2014, 07:21:58 AM

#### kaneyja

##### Re: ABC 3 point BASE measurement. How does algorithm works?
i calcaulated rotation matrix... like below
0.073349    0.997427    0.079695
-1.046134    0.062383   -0.433822
0.450464   -0.004284    3.865506

but i read one more time, i can't understand how can i get A, B, C value.

Today at 01:09:09 PM

Guest
##### Re:  ABC 3 point BASE measurement. How does algorithm works?

December 26, 2014, 07:45:47 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
now you have:

cos(A)*cos(B)=0.073349
sin(A)*cos(B)=-1.046134
-sin(B)=0.450464

and others, did you understand the logic?

so you just need to find:
B from -sin(B)=0.450464
than if you have B, you can find A :sin(A)*cos(B)=-1.046134
and finally if you have B, you can find C: cos(B)*sin(C)=-0.004284
« Last Edit: December 26, 2014, 08:21:55 AM by Mikail »

December 26, 2014, 07:59:52 AM

#### kaneyja

##### Re: ABC 3 point BASE measurement. How does algorithm works?
you're right!
but as you know sin , cos value has several values.
so i can't decide what value is correct.

in my case.

B value is 206.77345760534847393051262491475 or 333.22654239465152606948737508525 or
-153.22654239465152606948737508525 or -26.77345760534847393051262491475
C =   2.7503460678655562714068002632689 or -2.7503460678655562714068002632689
A = 94.712540033588601703511414758019 or 85.287459966411398296488585241981 or
94.712540033588601703511414758019 or 85.287459966411398296488585241981
« Last Edit: December 26, 2014, 08:03:31 AM by kaneyja »

December 26, 2014, 08:17:57 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
How they have several values?

When you are using on your calculator sin^(-1) you will get only 1 value.
For example
-sin(B)= 0.450464;
sin^(-1)(-0.450464) = -26.77345761 degrees
« Last Edit: December 26, 2014, 08:22:44 AM by Mikail »

December 26, 2014, 08:31:04 AM

#### kaneyja

##### Re: ABC 3 point BASE measurement. How does algorithm works?
but
-sin(B) = 0.450464
so B has several value don't you?

and i used sin^(-1), i will get only 1 value .
but this value has Condition like ( 180/2 degree < B <0 degree)
i really Confused.

December 26, 2014, 09:13:55 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
my friend you are mixing something. Sin and cos have only one values in each point. Think yourself, sin(pi/2)=0; sin(pi)=1/2; etc. Look to the graph that you sent before and try to find this values

December 26, 2014, 09:27:30 AM

#### hermann

##### Re: ABC 3 point BASE measurement. How does algorithm works?

December 26, 2014, 09:40:37 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
hermann, this attachment is not accessible

Today at 01:09:09 PM

Guest
##### Re:  ABC 3 point BASE measurement. How does algorithm works?

December 26, 2014, 09:56:31 AM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
mentioned results cannot be right...

sin and cos functions can NEVER have value that is greater than 1 or smaller than -1.
and when you multiply such values (magnitude of each one is <=1), result will only get smaller - there is just no way to get values of magnitude like 1.046 or 3.866 (positive or negative, does not matter).

also, not every matrix is a valid rotation matrix:

rotation matrix MUST be invertible (determinant not zero).
every element of rotation matrix MUST have value in range [-1,1]..
rotation matrix MUST be right hand and orthonormal (unless academic case), meaning that norm of
every row and every column, MUST be equal to 1 (otherwise matrix not just rotates
but also scales rotated vectors which is not valid, they must retain same length).

so when I tried values from points:
Origin   194.380200   1665.665000   1041.952000
x axis   201.715089   1561.051559   1086.998435
y axis   294.122864   1671.903273   1041.523605

my rotation matrix became:
0.064   0.956   -0.286
-0.917   0.170    0.362
0.395   0.239    0.887

Note that left column (i') is normalized P12; right column (k') is normalized P12xP13; and
center column (j') is just cross product of those two (of course order of multiplication matters).

if my program does not lie, result should work out to
base_data[1]={X 194.3802, Y 1665.665, Z 1041.952, A -85.989, B -23.246, C 15.097}

December 26, 2014, 12:18:26 PM

#### hermann

##### Re: ABC 3 point BASE measurement. How does algorithm works?
So here is the attachement.

December 26, 2014, 03:07:38 PM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
hermann, which input is required to perform the calculation? Is this only PT_IN_WORLD[] matrix that include 3 points? Thank you
« Last Edit: December 26, 2014, 03:35:51 PM by Mikail »

December 26, 2014, 04:11:30 PM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
you need to declare array of three FRAME

December 26, 2014, 05:11:47 PM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?

oh boy...
blindly trusting calculator is sure to bite back (and more often than you think...)

all periodic functions eventually have same result over and over - because they are periodic!
for example SIN(-210), SIN(30), SIN(150) and SIN(390) etc all have same result: 0.5. but when using calculator to compute inverse,
you only get ONE result which is 30deg. I hope you realize that 30 and 150deg are very different.
calculators are not very smart and just because you get result it does not mean it is a correct one.
YOU are supposed to then reason out which of the values may make sense and accordingly make adjustments by hand
(select result from proper quadrant if you have enough of data).

SIN is positive in quadrants 1 and 2
COS is positive in quadrants 1 and 4

TAN=SIN/COS

so you get positive result in both quadrants 1 (sin/cos both positive) and 3 (sin/cos both negative)
ATAN does not distinguish between the two and you need to adjust result by hand (if and when you can deduce correct one).
this is why in some cases (like in KRL) there is also ATAN2 which takes two arguments...

I am sure any calculator will also tell that square root of 4 is 2, but that is not correct either. there are two possible solutions:
(2)*(2)=4
(-2)*(-2)=4

I am pretty sure you would not want robot axis to go to +2meter position if the intention was to go to -2meter.
The same would be sending some axis to 30 instead of 150deg

December 27, 2014, 08:47:41 AM

#### ShaM

##### Re: ABC 3 point BASE measurement. How does algorithm works?
panic mode, thank you

otherwise, which SIN(-210), SIN(150) and SIN(390) you are talking about?  we are talking only about base ABC position, not robot! Can you imagine that your base will be turned 150 or -210 degree in any direction???
Of course I just need to choose smallest value, where the calculators result will be enought

anyway, thank you again
« Last Edit: December 27, 2014, 08:55:57 AM by Mikail »

December 27, 2014, 07:18:10 PM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
I posted one approach for computing ABC from positional data. idea is to form rotation matrix and then find ABC that produce those matrix coefficients.
the point is that when doing last step (angle computations) there are more than one way to choose angle values.

while choosing one angle value can be arbitrary, choosing arbitrary set of all three angles is not. this is what kaneyja run into (I think he went first for negative SIN(B) which is single term), but there are two possible angles. using ASIN function and blindly picking lower value can work in some cases but not always (depends on input data values of P1,P2,P3). this is why in KRL program that was linked by Hermann, another strategy is used (that code is clever and makes use of ATAN2 where possible etc.).

btw when I tried that code it did not work for me because of way parameters are passed, this is why I posted modified version along with test program in reply 17.
« Last Edit: December 27, 2014, 07:24:31 PM by panic mode »

Today at 01:09:09 PM

Guest
##### Re:  ABC 3 point BASE measurement. How does algorithm works?

December 29, 2014, 04:22:04 AM

#### kaneyja

##### Re: ABC 3 point BASE measurement. How does algorithm works?
you're right. my rotation matrix is not right.
but i calculated rotation matrix from 4 points.

for example.
i have 4 points coordinate in CAD like below.
x      y      z
Oc   0      0      0
P1   -27.74   -139.7   3.0936
P2   -77.94   0.0      103.29
P3   3.0936   8.0607   3.0936

and i measure 4 points coordinate in Robot like below.
X      Y      Z
Or   194.3802   1665.665   1041.952
R1    53.25087   1684.637   1042.009
R2   189.3055   1743.708   1038.0
R3   295.6255   1699.796   1040.968

CAD data is very correct. but my measure data has a little error.
because i measure 4 points manually.

And then.
this data used like attached picture.
i calculate rotation matrix like attached picture for t11 ~ t33.

Do you understand? If not, my explain is very poor..

but my calculate is not correct...

thanks.

December 30, 2014, 11:47:07 AM

#### panic mode

Global Moderator
##### Re: ABC 3 point BASE measurement. How does algorithm works?
not quite sure what are you trying to do. if i am not mistaken you want to choose couple of arbitrary points on some object (all coordinates are non-zero, perhaps amorphous object?) and express their position in robot coordinates, then establish frame/base for it?

this topic was about creating base from three points mimicking '3point' base measurement.
those three points are very specific, all values are in WORLD coordinate system and points are:
1. origin of base (XYZ are zero)
2. point on X axis (positive X; Y and Z are zero)
3. point on XY plane (positive Y; Z is zero)

so called 'indirect' base measurement uses 4 points which is what you seem to be after.
you may want to check forum for similar discussions, for example:
http://www.robot-forum.com/robotforum/robot-geometry-linear-algebra-forward-and-inverse-kinematics/best-fitindirect-measurement/

### Similar Topics

###### Teach point in defined BASE

Started by JackS on KUKA Robot Forum

4 Replies
155 Views
May 17, 2019, 06:56:51 PM
by panic mode
###### Position values of point in another base

Started by Plc_User on KUKA Robot Forum

3 Replies
1182 Views
March 09, 2016, 03:30:17 PM
by panic mode
###### 3-point method to create BASE

Started by BluesMatt on KUKA Robot Forum

3 Replies
3138 Views
December 30, 2014, 04:04:59 PM
by panic mode
###### Kuka iiwa r800 : Calibrating a base in the workbench with 3 point

Started by Magik07 on KUKA LBR IIWA

11 Replies
3331 Views
June 13, 2017, 01:02:03 PM
by greengrape83