Halcon中的三维位姿变换实现公转与自转效果（有代码）

        本文展示了这样一种效果：多个工件围绕一个大的圆盘进行转动的同时，自身还进行了一个自转，自转幅度与公转幅度相等。

        这里的每一个小工件的轴心方向并不与大圆盘的轴心方向一致，而是略微的向内收缩，每个工件的自转是沿着各自的z轴进行的（蓝色），因此各个工件的位姿关系并不是只有平移项。

        为了实现这个效果，我们可以通过pose_compose算子：

1. 设定圆盘中心的坐标系为全局坐标系Pose0；

2. 将工件移动到圆盘正上方的初始位置，此时的工件位姿为Pose1，Pose1包含了平移和旋转；

3. 当工件转动角度theta时，创建转动位姿PoseRotate := [0,0,0,0,0,theta,0]

4. 加入公转运动后的工件位姿为pose2 := pose_compose([0,0,0,0,0,theta,0],Pose1 )

5. 加入自转的工件位姿为pose_compose（pose2 ，[0,0,0,0,0,-1*theta,0]）

        解析：pose_compose（PoseL, PoseR）的操作用于将两个位姿合并，其效果等价与先对PoseL进行刚性变换，在对PoseR进行刚性变换；回到本案例，我们所最终得到的工件位姿等价于依次进行变换： [0,0,0,0,0,theta,0]，Pose1，[0,0,0,0,0,-1*theta,0]。每一次的刚性变换都是在新的坐标系中进行的，因此，这系列的操作等价于先在全局坐标系的中心旋转，之后沿旋转过的方向进行平移，最后再在平移的终点进行自转。

        代码如下：

    tuple_gen_sequence (0, 80, 1, Sequence)
    tuple_gen_const (81, 'color_', Newtuple)
    tuple_add (Newtuple, Sequence, colorName)

    colorValue := ['orange']    
    
    r := [O3dJL]
   *从-30度开始生成,跟踪第一个的位姿
   PoseWatch := []
    degree0 := Index - 30
    for Index1 := 0 to 15 by 1
        degree := degree0 + 360/15*Index1
        rigid_trans_object_model_3d (O3dDZ0, [0,0,0,0,0,degree,0], O3dDZ)
        
        *左边compose为公转，以坐标原点为变换原点
        pose_compose ([0,0,0,0,0,degree,0], PoseYL0, PoseYL)
        *右边compose为自转，以当前pose为变换原点
        pose_compose (PoseYL, [0,0,0,0,0,-1*degree,0], PoseYL)
        
        rigid_trans_object_model_3d (OriO3dYL, PoseYL, O3dYL)
        rigid_trans_object_model_3d (OriO3dArrow, PoseYL, O3dArrow)  
        
        *添加03d模型
        r := [r, O3dDZ,O3dYL,O3dArrow]
        *添加模型颜色，突出显示其中的第一个
        if(Index1 == 0)
            colorValue := [colorValue,'cyan','violet','red','green','blue']
            PoseWatch := PoseYL
        else
            colorValue := [colorValue,'cyan','yellow','red','green','blue']
        endif
        
    endfor
      
    if(Index == 0)
        visualize_object_model_3d(WindowHandle, r, ['area_scan_division',0.012,0.0,5e-6,5e-6,320,240,640,480], Pose, [colorName,'disp_pose'], [colorValue,'true'], [], [], [], PoseOut)
        write_pose (PoseOut, 'visualizePose.tup')
    endif
    
    disp_object_model_3d (WindowHandle, r, ['area_scan_division',0.012,0.0,5e-6,5e-6,320,240,640,480], PoseOut, [colorName,'disp_pose','disp_background'], [colorValue,'true','false'])