nope/modules/mod-Transformation-Tree/test/test-tree.ts

/**
 * @author Martin Karkowski
 * @email m.karkowski@zema.de
 * @create date 2018-06-15 12:11:22
 * @modify date 2020-02-28 16:14:44
 * @desc [description]
*/

/** Clear the Screen */


import 'reflect-metadata';
import { Builder } from '../../mod-Assembly-Builder/src/Container-Builder.FileLoader';
import { rgetattr, rsetattr } from '../../mod-TypeScript-Library/src/Object-Methods';
import * as CS from '../assembly/manual-assembly';
import * as geometry from '../src/geometric-functions';




function compare(orignal, adapted) {

    const v1 = geometry.transformationToPose(orignal);
    const v2 = geometry.transformationToPose(adapted);

    const path = [
        'trans.x',
        'trans.y',
        'trans.z',
        'orientation.imgX',
        'orientation.imgY',
        'orientation.imgZ',
        'orientation.real',
    ]

    const _comp = {};

    for (const p of path) {

        const _new = rgetattr(v1, p);
        const _org = rgetattr(v2, p);

        rsetattr(_comp, p, {
            old: _org,
            new: _new,
            diff: _org - _new
        });

    }


    console.log('DIFFERENCE:\n', _comp);
}

Builder.load();

Builder.on('loaded', () => {

    const _cs = Builder.instance.container.get<CS.CoordinateContainer>(CS.TYPES.CoordinateContainer);

    /** Add Root CS */
    _cs.addCoordinateSystem('root', null);

    _cs.addCoordinateSystem('robot', 'root', geometry.homogenousCoordinates({ x: 0, y: 0, z: 0 }), 'fromParent');

    _cs.addCoordinateSystem('tcp', 'robot', geometry.homogenousCoordinates(
        {
            "x": 0.1639767602561366,
            "y": -0.8244141868788568,
            "z": 0.09691757147957357
        },
        {
            "real": 0.4978925975514955,
            "imgX": 0.5034369823164051,
            "imgY": -0.5020103892132208,
            "imgZ": 0.49662836735487315
        }
    ), 'toParent');

    _cs.addCoordinateSystem('camera', 'tcp', geometry.homogenousCoordinates(
        {
            x: 0.3822017,
            y: 0.0021587,
            z: 0.022724
        },
        {
            imgX: 0,
            imgY: -1,
            imgZ: 0,
            real: 1.570796
        }
    ), 'toParent');

    _cs.addCoordinateSystem('tool', 'tcp', geometry.homogenousCoordinates(
        {
            x: 0.3822017,
            y: 0.002144,
            z: 0.0932456
        }, {
        imgX: 0,
        imgY: -1,
        imgZ: 0,
        real: 1.570796
    }
    ), 'toParent');

    const _start = _cs.transform('robot', 'tcp', geometry.poseToTransformation({
        orientation: geometry.UNIT_QUAT,
        trans: { x: 0, y: 0, z: 0.0 }
    }));

    console.log('ORIGINAL:\n', geometry.transformationToPose(_start));

    const res = _cs.multiTransform({ src: 'camera', dst: 'tool' }, 'robot', 'tcp', geometry.poseToTransformation({
        trans: {
            x: 0,
            y: 0,
            z: 0.05
        }
    }))
    console.log('Point in Robot-Base:\n', geometry.transformationToPose(res));

    compare(_start, res);


    // console.log('Length-Delta trans:\n', Math.sqrt(Math.pow(_diff.trans.x,2)+Math.pow(_diff.trans.y,2)+Math.pow(_diff.trans.z,2)));
});