异读：工硕士呆板人本领有限公司动作智能工场效劳商，从智能创造处置计划的供给，到产业电气产物的一站式购买，从智能创造培养和训练到专科保护珍爱，环绕摆设的智能晋级变革、消费加工、摆设保护、本领培养和训练等多个上面，实行消费的智能、高效、环境保护和安定。不妨接洽（一...

工硕士呆板人本领有限公司动作智能工场效劳商，从智能创造处置计划的供给，到产业电气产物的一站式购买，从智能创造培养和训练到专科保护珍爱，环绕摆设的智能晋级变革、消费加工、摆设保护、本领培养和训练等多个上面，实行消费的智能、高效、环境保护和安定。不妨接洽（一九九，二一二七，二六六五）

1ABB呆板人坐标系ABB呆板人编制程序示教的原点不妨经过上述三个点创造，个中X1到x2形成X目标，从Y1到x1x2的笔直线是Y目标，Z目标满意右手规则，即经过X目标乘以Y目标赢得Z目标。

2我报告过你怎样从三点坐标x1x2和Y1计划原点o。

3正文引见了怎样经过x1x2和Y1计划坐标系原点o的目标。也不妨运用ABB呆板人因变量orient ZY x将Euler观点变换为四元数Q1-Q4。

计划本领如次

1） 获得向量和x1x2的单元化

2） 获得向量x1y1并对其举行单元演算

3） 单元向量Oz即是单元向量x1x2叉积单元向量x1y1

4） 单元向量yy1即是单元向量yy1与单元向量oz的交加abb变频器妨碍代码的乘积。

5） 获得单元向量OX1，单元向量OX1，oy1，单元向量，单元向量英两

6） 将回旋矩阵OX1，oy1，OZT变幻为欧拉角

7） 用目标因变量实行欧拉角到四元数的变换

4创造步调calframe定向，输出参数pX1、pX2和py，输入到ABC（即ABB呆板人RZ、ry和RZ对应的欧拉角ABC）

代码如次：

进程校准框架定向（场所pX1、pX2、场所pY、输出编号a、输出编号B、输出编号C）

! A:ABB呆板人编制程序示教回旋z（），B:回旋y（），C:回旋x（）

VAR场所vpx；

VAR场所vpxy；

VAR场所vpy；

VAR场所vpz；

vpx:=px2-px1；

! 获得向量x1x2

VpX:=vec_u2;nor（VpX）；！单元向量VpX

vpxyx/pyx/px1！获得向量x1y1

vpxy:=vec_uu2;nor（vpxy）；！矢量VpX单元

Vpz:=VpXvpxy；！向量VpX叉积向量vpxy

Vpzabb变频器妨碍代码vpx；！向量叉积向量

! 在数组NRTabb变频器妨碍代码中保存单元向量

NRT：=vpx.x；

nrT：=vpy.x

1,3}：vpz.x；

NRT：=VpX.Y；

NRT：=vpy.y；

nrT：=vpz.y

NRT：=vpx.z；

NRT：=Vpy.z；

NRT：=vpz.z；

矩阵NRT，a，B，C；

! 挪用回旋矩阵的欧拉角因变量

中断步调

procABB呆板人编制程序示教矩阵样式（num NRT{，}，InOut num NRA，InOut num NRB，InOut num NRC）

! 交通角变幻矩阵A，B，C

! t=回旋Z（a）回旋y（b）回旋x（c）

变异数nrSinA；

nrCosA变量；

变量nrSinB；

Var num nrabscosb；变量数nrabscosb；

变量nrSinC；

无功积累；

NRA:=ATanabb变频器妨碍代码2（NRT，NRT）；

nrSinA:=SIN（NRA）；

NrCosA:=Cos（nrA）；

3,1} ;

NRABSCOSB:=NRCOSANRTNRSNANRT；

! 值：-90

nrB:=ATan2（nrSinB，nrAbsCosB）

Nrsinc:nrsinanrt-nrcosanrt；

NRCOSC:=-NRSNANRTABB呆板人编制程序示教NRCOSANRT；

NRC：abb变频器妨碍代码=ATAN2（nrSinC，nrCosC）；

中断步调

功效场所向量（场所1）

VAR pos场所2；

posx:=posx/VectMagn（pos1）

posy:=posy/VectMagn（pos1）

pOSz:=posz/VecMagn（pOS 1）；

归来pos2；

ENDFUNC公司

ABB呆板人