学习笔记31 --编写urdf时关于复杂rpy计算技巧分享

urdf使用环境：Ubuntu16.04 ros-kinetic gazebo
rpy计算环境：window10 solidwork19 matlab17b

问题来源：
根据机器人sw文件，编写urdf时候，每个link都会要求填写xyzrpy坐标，xyz的值还是比较好算的，但是这个rpy就不简单了。如果属于单纯某一轴转90°来写还简单，但是对于相机这类对安装要求苛刻的的link编写时候，这个rpy填写就不容易了。

难点在于，rpy属于欧拉角，填写urdf时候写的是其的弧度制形式欧拉角；对于欧拉角，个人理解，欧拉角并不是简单从sw装配图里面度量其线面成角，而是一个每次绕轴转角度都会对下次绕轴旋转产生影响（产生分量）的角度旋转，可以从物理力分量角度类似理解。

那么，在开始之前，普及一个知识点，urdf里面的欧拉角旋转顺序是zyx，也就是先绕z轴转动，然后绕y轴转动，最后绕这x轴旋转的。

通过一番研究以及前辈指导，获取一条重要信息，得到目标link的旋转矩阵，就可以利用ros里面tf包有计算rpy的函数进行计算（考虑到sw这些都是再window这边的，后面采用方式是，matlab17b计算）。

解决的基本方式：
在sw里面构建一个模型，把该部件的安装位置描述出来；然后创建一个参考坐标系，表示该部件link的所处的范围；然后通过一个宏定义，计算出部件的原点相对于新建参考坐标系的旋转矩阵；把旋转矩阵拷贝到matlab里面，计算旋转矩阵的逆矩阵，然后计算逆矩阵的欧拉角；得到的欧拉角就是我们需要的rpy值。

下面以一个部件安装位置（安装台是与oxy面成角45°，在此基础上往上翘45°，重点获取与部件直接相连板的空间位置），说面其欧拉角rpy的计算过程。

（1）要注意零件原点[这里假设base_link就是位于该原点上]（右侧位置xy指向点）以及坐标系（左侧xy指向坐标系），一定一定要和urdf里面的xy定义指向一致；一般选择 前视基准面 为第一次草图绘制基准面；

（2）由于安装台位置与z轴成角45°，所以创建了下面的草图；

（3）由于安装台在倾斜45°后面在往上翘45°，所以建立建图结果图如下图示；

（4）选择第一个创建平面为草图平面，平行x、y创建平面，目的是为了获取在步骤（3）创建的面，三面相交的点就是表示部件角度关系安装位置（该点就是部件的安装位置关于旋转角的样式：机械狗一般知道，任何机械装配体都是有一定的安装位置，由各自部件的轮廓关系堆叠出来的，而我就是根据这个原理来搭建模型）；

（5）“插入-参考几何体-坐标系”，在左侧弹出窗口选择该点，xyz的选择根据部件的link的xyz方位来选择。好了，创建出 坐标系1，如下图示。

（6）依次点击“坐标系1–宏--新建宏–（定义宏名称选择保存位置）–（拷贝代码进去）–运行–运行宏"–在立即窗口（ctrl+g 可以调用出来或者 视图–立即窗口 调用出来）输出原点相对坐标系1的旋转矩阵；

宏代码（源自sw官网）：

Dim swApp As Object
Sub main()

    Set swApp = Application.SldWorks
    
    Dim swModel As SldWorks.ModelDoc2
    
    Set swModel = swApp.ActiveDoc
    
    Set swMathUtil = swApp.GetMathUtility
    
    If Not swModel Is Nothing Then
        
        Dim swFeat As SldWorks.Feature
        Set swFeat = swModel.SelectionManager.GetSelectedObject6(1, -1)
        
        If Not swFeat Is Nothing Then
            
            If swFeat.GetTypeName2() = "CoordSys" Then
                
                Dim swCoordSys As SldWorks.CoordinateSystemFeatureData
                Set swCoordSys = swFeat.GetDefinition
                Dim swMathTransform As SldWorks.MathTransform
                Set swMathTransform = swCoordSys.Transform
                
                Dim vMatrix As Variant
                vMatrix = swMathTransform.ArrayData
                
                Debug.Print vMatrix(0) & "," & vMatrix(1) & "," & vMatrix(2) & ","
                Debug.Print vMatrix(3) & "," & vMatrix(4) & "," & vMatrix(5) & ","
                Debug.Print vMatrix(6) & "," & vMatrix(7) & "," & vMatrix(8) & ","
                
                
            Else
                MsgBox "Selected feature is not a coordinate system"
            End If
        Else
            MsgBox "Please select coordinate system feature"
        End If  
    Else
        MsgBox "Please open model"
    End If 
End Sub

（7）拷贝旋转矩阵（sw宏插件里面的 立即窗口 输出的矩阵）到matlab17b；计算该旋转矩阵的逆矩阵，该逆矩阵就是坐标系1相对部件原点（假设urdf里面的base_link是部件原点，而部件link是连接到到base_link上面的，然后目标部件link的角度位置相当于坐标系1位置关系） 的旋转矩阵；使用matlab自带的公式（rpy旋转矩阵欧拉角计算解析https://blog.csdn.net/qq_45701501/article/details/107243415）计算得到欧拉角r= 2.1863（绕x轴转动弧度）、p= -0.5236（绕y轴转动弧度）、y= -0.9553 （绕z轴转动弧度）。

（8）总结：
a、这个方法，适合对于sw二次开发不懂，但又有urdf复杂的rpy计算需求的同学；
b、对sw的坐标系理解明白，然后对所创建的机器人的urdf坐标系也很理解情况下进行的；这保证sw的建模的坐标系和机器人urdf在角度装配关系上配合一致；
c、会基本的matlab公式计算。
d、建模-sw求初步旋转矩阵-matlab求目标旋转矩阵-matlab求欧拉角

####################
好记性不如烂笔头
'–20200726