127 lines
3.1 KiB
TypeScript
127 lines
3.1 KiB
TypeScript
/**
|
|
* @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 * as CS from '../assembly/manual-assembly';
|
|
import * as geometry from '../src/geometric-functions';
|
|
|
|
import { Builder } from '../../ZISS-Assembly-Builder/src/Container-Builder.FileLoader';
|
|
import { rgetattr, rsetattr } from '../../ZISS-TypeScript-Library/src/Object-Methods';
|
|
|
|
|
|
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)));
|
|
}); |