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

0 Members and 1 Guest are viewing this topic.

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
ABC 3 point BASE measurement. How does algorithm works?
« on: December 25, 2014, 08:01:26 AM »
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?

Offline panic mode

  • Global Moderator
  • Hero Member
  • *****
  • Thank You
  • -Given: 77
  • -Receive: 789
  • Posts: 4897
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #1 on: December 25, 2014, 06:36:48 PM »
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
A is rotation about Z
B is rotation about Y
C is rotation about X

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  :cheer:)

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 »
1) http://www.robot-forum.com/robotforum/kuka-robot-forum/read-first/
2) if you want reply about robot, post it in forum
3) read 1 and 2

Offline panic mode

  • Global Moderator
  • Hero Member
  • *****
  • Thank You
  • -Given: 77
  • -Receive: 789
  • Posts: 4897
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #2 on: December 25, 2014, 06:50:36 PM »
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)
1) http://www.robot-forum.com/robotforum/kuka-robot-forum/read-first/
2) if you want reply about robot, post it in forum
3) read 1 and 2

Offline kaneyja

  • Jr. Member
  • **
  • Thank You
  • -Given: 5
  • -Receive: 0
  • Posts: 56
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #3 on: December 26, 2014, 06:12:07 AM »
hello panic mode !
i really really need your knowledge.
but i can't understand .... :waffen100:

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...
please help me :help:


Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #4 on: December 26, 2014, 06:52:23 AM »
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?

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #5 on: December 26, 2014, 07:01:40 AM »
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

Offline kaneyja

  • Jr. Member
  • **
  • Thank You
  • -Given: 5
  • -Receive: 0
  • Posts: 56
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #6 on: December 26, 2014, 07:21:58 AM »
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.

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #7 on: December 26, 2014, 07:45:47 AM »
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 »

Offline kaneyja

  • Jr. Member
  • **
  • Thank You
  • -Given: 5
  • -Receive: 0
  • Posts: 56
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #8 on: December 26, 2014, 07:59:52 AM »
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 »

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #9 on: December 26, 2014, 08:17:57 AM »
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 »

Offline kaneyja

  • Jr. Member
  • **
  • Thank You
  • -Given: 5
  • -Receive: 0
  • Posts: 56
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #10 on: December 26, 2014, 08:31:04 AM »
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.

Please save me   :bawling:

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #11 on: December 26, 2014, 09:13:55 AM »
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

Offline hermann

  • Sr. Member
  • ****
  • Thank You
  • -Given: 1
  • -Receive: 76
  • Posts: 345

Offline ShaM

  • Full Member
  • ***
  • Thank You
  • -Given: 11
  • -Receive: 4
  • Posts: 102
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #13 on: December 26, 2014, 09:40:37 AM »
hermann, this attachment is not accessible

Offline panic mode

  • Global Moderator
  • Hero Member
  • *****
  • Thank You
  • -Given: 77
  • -Receive: 789
  • Posts: 4897
Re: ABC 3 point BASE measurement. How does algorithm works?
« Reply #14 on: December 26, 2014, 09:56:31 AM »
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}
1) http://www.robot-forum.com/robotforum/kuka-robot-forum/read-first/
2) if you want reply about robot, post it in forum
3) read 1 and 2