prevent gimbal lock on angle-axis transformation w

2019-08-19 05:19发布

I would like to put a sticker on the surface of target. I use a depth cam and a robotics arm.

The setting is like this. enter image description here

I know there will be gimbal lock on y axis because the target is parallel to z axis.

M1: cam2targetT:
 0.933286 -0.358041 0.0279849 -0.250368
-0.162293 -0.489985  -0.85649 0.0660773
 0.320371  0.794808 -0.515404   1.03789
        0         0         0         1

M2: arm2camT:
        0  0.707107  0.707107       0.1
        1         0         0      0.53
        0  0.707107 -0.707107      0.96
        0         0         0         1

Mf: arm2tagetT:
 0.111778  0.215542 -0.970075  0.880623
 0.933286 -0.358041 0.0279849  0.279632
-0.341295 -0.908486 -0.241184  0.272825
        0         0         0         1

When i send the command to UR hand, it is in this format. I used this

Eigen::AngleAxisd newAngleAxis(mat);
std::cout << newAngleAxis.angle() << "\n" << newAngleAxis.axis() << "\n\n";

and i got this: 3.87376 0.700441 0.470301 -0.536842

The final command to UR is this pose.

Mf in axis-angle T:
0.880623,0.279632,0.272825
AngleAxisd R:
2.71334,1.82184,-2.0796

And as expected, gimbal lock happened, so what question is, how can I find the correct angle-axis in this case? is the problem can be solved by quaternions and how?

P.S. I don't think I can change the rotation order in UR.

标签: unity3d math 3d
0条回答
登录 后发表回答