2. 运动时，J3电机带动J3连杆。3轴，J3连杆，上臂和后端连杆构成如图的平行四边形结构。

3. 所以，若2轴电机转动，但3轴电机不转动，此时3轴和大臂的夹角会变化（没有连杆的机器人，2轴电机转动，3轴电机不转动，3轴和大臂夹角不会变化）

以上两幅图片，3轴均为0度，但由于2轴转动，3轴与大臂夹角不一样

4. 对于不带连杆的机器人正运动学，可以通过dh参数转化为各个轴的位姿矩阵，将6个位姿矩阵右乘即可得到当前机器人末端的笛卡尔坐标，具体参见  机器人如何从各轴角度算出当前XYZ

5. 但对于带连杆机器人，2轴的变化会导致3轴与大臂夹角发生变化。由于平行四边形结构的缘故，实质3轴与大臂夹角变化刚好等于2轴变化角度的负数。

6. 故在位姿矩阵右乘时，只需在右乘到轴3 矩阵后，再右乘一个旋转矩阵，将夹角补偿回来即可。

7. 1410机器人DH参数如下

8. 针对1410类似带连杆机器人，机器人正运动学RAPID实现方法如下

 LOCALVAR num alpha{6}:=[0,-90,0,-90,90,90];

   LOCALVAR num a{6}:=[0,150,600,120,0,0];

   LOCALVAR num theta{6}:=[0,-90,0,0,-180,0];

LOCALVAR num d{6}:=[475,0,0,720,0,85];

!1410 机器人DH参数

VAR num no_dependent;

!3轴与大臂夹角

   PROC test_dh_pose()

       VAR num curr_angle{6}:=[0,0,0,0,0,0];

       VAR pose pose10{6};

       VAR pose pose_cal:=[[0,0,0],[1,0,0,0]];

       VAR jointtarget jtmp;

       jtmp:=CJointT();

       curr_angle{1}:=jtmp.robax.rax_1;

       curr_angle{2}:=jtmp.robax.rax_2;

       curr_angle{3}:=jtmp.robax.rax_3;

       curr_angle{4}:=jtmp.robax.rax_4;

       curr_angle{5}:=jtmp.robax.rax_5;

       curr_angle{6}:=jtmp.robax.rax_6;

       FOR i FROM 1 TO 6 DO

           IF i=3 THEN

               no_dependent:=-curr_angle{2};

             !夹角等于2轴运动的负数

           endif

           pose10{i}:=f_dh2pose(i,alpha{i},a{i},theta{i}+curr_angle{i},d{i});

       ENDFOR

       FOR i FROM 1 TO 6 DO

           pose_cal:=PoseMult(pose_cal,pose10{i});

       ENDFOR

ENDPROC

   FUNC pose f_dh2pose(num i,num alpha,num a,num theta,num d)

       VAR pose pose1:=[[0,0,0],[1,0,0,0]];

       pose1:=PoseMult(pose1,[[0,0,0],orientzyx(0,0,alpha)]);

       pose1:=PoseMult(pose1,[[a,0,0],orientzyx(0,0,0)]);

       pose1:=PoseMult(pose1,[[0,0,0],orientzyx(theta,0,0)]);

       pose1:=PoseMult(pose1,[[0,0,d],orientzyx(0,0,0)]);

      IF i=3 THEN

           pose1:=PoseMult(pose1,[[0,0,0],orientzyx(no_dependent,0,0)]);

       endif

       !针对轴3坐标系，需要再多乘一次旋转，将夹角补偿回来

       RETURN pose1;

   ENDFUNC