【机器人学】7-3.六自由度机器人自干涉检测-圆柱体的旋转变换【附MATLAB代码】

        前言

        上一章确定了机械臂等效的圆柱体的上下圆心坐标，这篇文章将解决算法三个核心中的第二个核心：

                一  根据机械臂的几何数据以及DH参数，确定机械臂等效的圆柱体的上下圆心坐标。

                二  将一个圆柱体旋转到与坐标Z轴对齐，另一个圆柱体转到，上下圆在XoY平面的

                     投影形成的椭圆在y方向上长轴为2r，这一个旋转流程的数学表达。

                三  原点与椭圆的关系，求原点是否在椭圆内部，原点到椭圆的最短距离，线段与

                     线段的最短距离。

        之所以要将圆柱体作旋转变换，是为了方便借用投影法分析。

        旋转的数学表达

        如式(7)所示，L2是围绕L1的下底圆心转动，故无论如何转动都不会改变两圆柱体在空间中的位置关系。

matalb代码

%input 两个圆柱的底面圆心坐标。
%output l2圆柱旋转后的上下底面圆心坐标，以及投影后椭圆的短轴与长轴之比。
function [p1,p2,beta] = TransferCylinder2XZ(p11,p12,p21,p22)
n1=p12-p11;
n1=n1/norm(n1);
nx=n1(1);
ny=n1(2);
nz=n1(3);
rx=atan2(ny,nz);
nz2=ny*sin(rx)+nz*cos(rx);
ry=-atan2(nx,nz2);


n2=p22-p21;
n2=n2/norm(n2);
cosRy=cos(ry);
sinRy=sin(ry);
cosRx=cos(rx);
sinRx=sin(rx);
rotyx=[cosRy sinRy*sinRx sinRy*cosRx
        0 cosRx -sinRx
        -sinRy cosRy*sinRx cosRy*cosRx];
n21=rotyx*n2;
rz=atan2(-n21(2),n21(1));

PT  = PoseTrans;
r01=PT.rotZ(rz)*rotyx;

p1=r01*(p21-p11);
p2=r01*(p22-p11);
beta=acos(r01(3,:)*n2);
end

    下一章：    【机器人学】7-4.六自由度机器人自干涉检测-两圆柱体空间关系【附MATLAB代码】-优快云博客https://blog.youkuaiyun.com/y12345655/article/details/141868931?spm=1001.2014.3001.5501