Kuka iiwa r800 : Calibrating a base in the workbench with 3 point

It seems that there is no such api in sunrise api. So I wrote something like the following as the base generator for our project.

/**
     * Generate a base frame from frame point a, b, c by right hand law. The base frame' z direction is the normal
     * direction of base abc.
     */
    public static Frame generateBaseFrame(AbstractFrame a, AbstractFrame b, AbstractFrame c)
    {
        Vector startZ = Vector.of(0.0, 0.0, 1.0);


        Vector direction = a.getOriginOf(b).toVector().crossProduct(a.getOriginOf(c).toVector());


        if (direction.equals(Vector.of(0.0, 0.0, 0.0)))
        {
            throw new IllegalArgumentException("These 3 frames can't produce a base frame.");
        }


        double rotAngle = startZ.angleRad(direction);


        Vector rotAxis = null;
        if (rotAngle == 0.0 || rotAngle == Math.PI)
        {
            rotAxis = Vector.of(1.0, 0.0, 0.0);
        }
        else
        {
            rotAxis = startZ.crossProduct(direction);
        }


        return new Frame(a, Transformation.of(Vector.of(0.0, 0.0, 0.0), AxisAngleRotation.of(rotAxis, rotAngle)));
    }

The whole process is
(1)get the vector ab and ac, where frame a as the reference coordinate system.
(2)check whether ab and ac are on the same line. If so, throw exception.
(3)ab cross ac is the normal direction of new base, where frame a as the reference coordinate system.
(4)calculate the angle from reference coordinate system's z vector(0,0,1) to the new base's normal direction by the right hand law.
I)if the z vector and the normal direction is on the same line(angle is 0 or Pie), choose any vector except z as the pivot to rotate around.
II) else choose the real pivot vector to rotate around. The pivot vector is the cross of the two vectors.
(5) value Transformation.of(Vector.of(0.0, 0.0, 0.0), AxisAngleRotation.of(rotAxis, rotAngle)) is the Transformation from frame a to the new base.
(6) So the final base frame should combine the reference frame a and the transformation together.
Welcome to any questions and suggestions.

Quote from Magik07

Thanks for answer me.
I understand what you mean until this moment :

double rotAngle = startZ.angleRad(direction);

Vector rotAxis = null;
if (rotAngle == 0.0 rotAngle == Math.PI)
{
rotAxis = Vector.of(1.0, 0.0, 0.0);
}
else
{
rotAxis = startZ.crossProduct(direction);
}

return new Frame(a, Transformation.of(Vector.of(0.0, 0.0, 0.0), AxisAngleRotation.of(rotAxis, rotAngle)));

I don't understand what are RotAngle and RotAxis, and i don't understand the return frame, can you explain more please ?
Sorry for my bad English.

Display More

I suspect that you have only printed the Transformation of the base frame relative to his parent(frame a).
You can get its transformation relative to World by method baseFrame.transformationFromWorld().
Also you can get the formula by the combination of cross multiplying and dot multiplying.
Maybe you can provide more details about your 3 frames/points so that I can have a verification.

Both bases are OK.
Firstly the x/y/z in the world are the same for the taught base and the calculated base.
Secondly the transformation between the two bases is [X=0.00 Y=0.00 Z=0.00 A=1.72 B=0.00 C=0.00] which means they have the same Z direction.

If you want to specify the x,y direction of you base, you need to specify more(like x goes along ab direction) because the right hand law only can decide Z direction

Frame a = new Frame(532.5, -160.7, -60.7, 0.2, -0.002, 3.1);
        Frame b = new Frame(532.5, -331.9, -60.7, 0.2, -0.002, 3.1);
        Frame c = new Frame(637.7, -160.6, -60.3, 0.2, -0.002, 3.1);
        Frame taughtBase = new Frame(532.5, -160.7, -60.7, -1.6, -0.001, 0.004);
        Frame calcBase = generateBaseFrame(a, b, c);
        System.out.println("calc          base:" + calcBase.toStringInWorld());
        System.out.println("base transform:" + taughtBase.staticTransformationTo(calcBase));
        //calc          base:[X=532.50 Y=-160.70 Z=-60.70 A=0.12 B=-0.00 C=-0.00]
        //base transform:[X=0.00 Y=0.00 Z=0.00 A=1.72 B=0.00 C=0.00]

You can use dot product to get an angle between two vectors.

I've made some adjustment of the base frame generating function, which makes a as original point, x axis along with ab, under right hand law.
Also I made the calculated base frame child of world directly instead of child of frame a.

/**
     * Generate a base frame from frame point a, b, c by right hand law. The base frame' z direction is the normal
     * direction of base abc, and x direction is same as ab.
     */
    public static Frame generateBaseFrame(AbstractFrame a, AbstractFrame b, AbstractFrame c)
    {
        Vector startZ = Vector.of(0.0, 0.0, 1.0);
        Vector xVector = Vector.of(1.0, 0.0, 0.0);


        Vector direction = a.getOriginOf(b).toVector().crossProduct(a.getOriginOf(c).toVector());


        if (direction.equals(Vector.of(0.0, 0.0, 0.0)))
        {
            throw new IllegalArgumentException("These 3 frames can't produce a base frame.");
        }


        double rotAngle = startZ.angleRad(direction);


        Vector rotAxis = null;
        if (rotAngle == 0.0 || rotAngle == Math.PI)
        {
            rotAxis = xVector;
        }
        else
        {
            rotAxis = startZ.crossProduct(direction);
        }
        Frame tmpZbase = new Frame(a, Transformation.of(Vector.of(0.0, 0.0, 0.0), AxisAngleRotation.of(rotAxis, rotAngle)));
        Vector abInbase = tmpZbase.getOriginOf(b).subtract(tmpZbase.getOriginOf(a));
        double rotAngleInbase = xVector.angleRad(abInbase);
        if (rotAngleInbase == 0.0 || rotAngleInbase == Math.PI)
        {
        }
        else
        {
            if (xVector.crossProduct(abInbase).getZ() < 0.0d)
            {
                rotAngleInbase = -rotAngleInbase;
            }
        }
        return new Frame(tmpZbase.transform(Transformation.ofRad(0.0, 0.0, 0.0, rotAngleInbase, 0.0, 0.0)).transformationFromWorld());
    }