Posts by Spl



Ah, you probably need a PID controller calculate the corrections.

Why don't you send the axis positions to your c# programs as feedback and calculate the corrections from them?

Read page 80 of the expert programming manual.

The format would depend on what you want to accomplish, but sending KRL commands isn't possible as far as I know.

Isn't WorkVisual free? If you don't have it, just send email to KUKA.

How did you verify it's not moving the 100mm in the new frame?


The cartesian coordinates you see depend on what tool correction you are using.

Here's the full code that gives the correct answer
double a, b, c;
double sx, sy, sz;
double cx, cy, cz;a = 0.0 * Math.PI / 180.0;
b = 0.0 * Math.PI / 180.0;
c = 0.0 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D base_old = new Matrix3D(cy * cz, cz * sx * sy  cx * sz, cx * cz * sy + sx * sz, 0,
cy * sz, cx * cz + sx * sy * sz, cz * sx + cx * sy * sz, 1126.5,
sy, cy * sx, cx * cy, 805.0,
0, 0, 0, 1);a = 180.0 * Math.PI / 180.0;
b = 81.6634 * Math.PI / 180.0;
c = 180.0 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D pose_old = new Matrix3D(cy * cz, cz * sx * sy  cx * sz, cx * cz * sy + sx * sz, 1490.46,
cy * sz, cx * cz + sx * sy * sz, cz * sx + cx * sy * sz, 1126.5,
sy, cy * sx, cx * cy, 1459.4,
0, 0, 0, 1);a = 3.2965 * Math.PI / 180.0;
b = 1.9967 * Math.PI / 180.0;
c = 0.115 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D base_new = new Matrix3D(cy * cz, cz * sx * sy  cx * sz, cx * cz * sy + sx * sz, 49.907,
cy * sz, cx * cz + sx * sy * sz, cz * sx + cx * sy * sz, 1060.275,
sy, cy * sx, cx * cy, 791.113,
0, 0, 0, 1);base_new.Invert();
Matrix3D pose_new = System.Windows.Media.Media3D.Matrix3D.Multiply(base_new,base_old);
pose_new = System.Windows.Media.Media3D.Matrix3D.Multiply(pose_new, pose_old);// Convert rotation matrix to abc angles
double a_new, b_new, c_new;
if (Math.Abs(pose_new.M31 1) < 1e6)
{
a_new = Math.Atan2(pose_new.M23,pose_new.M22);
b_new = Math.PI / 2;
c_new = 0.0;
}
else if (Math.Abs(pose_new.M31 +1) < 1e6)
{
a_new = Math.Atan2(pose_new.M23, pose_new.M22);
b_new = Math.PI / 2;
c_new = 0.0;
}
else
{
a_new = Math.Atan2(pose_new.M21, pose_new.M11);
b_new = Math.Asin(pose_new.M31);
c_new = Math.Atan2(pose_new.M32, pose_new.M33);
}a_new *= 180 / Math.PI;
b_new *= 180 / Math.PI;
c_new *= 180 / Math.PI; 
Try with
cos(b)*cos(a) cos(a)*sin(c)*sin(b)cos(c)*sin(a) cos(c)*cos(a)*sin(b)+sin(c)*sin(a)
cos(b)*sin(a) cos(c)*cos(a)+sin(c)*sin(b)*sin(a) cos(a)*sin(c)+cos(c)*sin(b)*sin(a)
sin(b) cos(b)*sin(c) cos(c)*cos(b)Your current rotation matrix seems to be transposed.

Compare all the matrices with the ones I posted earlier and see where the difference comes from.

I'm using the following C++ code with the Eigen libraries http://eigen.tuxfamily.org/index.php?title=Main_Page
Vector3f e;
e << 180.0, 81.6634, 180.0;
e *= M_PI / 180.0;float sx, sy, sz;
float cx, cy, cz;sx = sin(e(2)), sy = sin(e(1)), sz = sin(e(0));
cx = cos(e(2)), cy = cos(e(1)), cz = cos(e(0));Matrix3f ret;
ret << cy*cz, cz*sx*sy  cx*sz, cx*cz*sy + sx*sz,
cy*sz, cx*cz + sx*sy*sz, cz*sx + cx*sy*sz,
sy, cy*sx, cx*cy;Hope this helps you.

Ah, of course the KRL functions aren't the same, but I wasn't think of the KRL inverse function.

I have never used it, but I would guess so.

0.997739 0.0574681 0.0348419 16.4261
0.0574331 0.998348 0.00200591 1062.98
0.0348996 2.92901e07 0.999391 788.889
0 0 0 1*
1 0 0 0
0 1 0 1126.5
0 0 1 805
0 0 0 1*
0.144988 9.2377e10 0.989433 1490.46
1.26753e08 1 9.2377e10 1126.5
0.989433 1.26753e08 0.144988 1459.4
0 0 0 1=
0.110187 0.0574681 0.992248 1424.62
0.0063424 0.998348 0.0571171 1144.04
0.993891 3.05601e07 0.110369 1526.15
0 0 0 1=
1424.62 1144.04 1526.15 176.706 83.6635 180
1st matrix is the inverse of the new base
2nd matrix is the old base
3rd matrix is the pose in old base
4th matrix is the pose in the new base 
To solve a pose in base 2, you have to solve the equation wHb1*b1Hp = wHb2*b2Hp, from which you get b2Hp = inverse(wHb2)*wHb1*b1Hp.
wHb1 = 4by4 matrix of your base 1 in world
wHb2 = 4by4 matrix of your base 2 in world
b1Hp = 4by4 matrix of your pose in base 1
b2Hp = 4by4 matrix of your pose in base 2 

BASE_DATA[17].Z= BASE_DATA[17].Z + 1 works only if the euler angles are zero, so I would recommend to use the geometric operator.
BASE_DATA[17] = BASE_DATA[17]:{X 0, Y 0, Z 1, A 0, B 0, C 0}