異讀：工碩士呆板人本領有限公司動作智能工場效勞商，從智能創造處置計劃的供給，到產業電氣產物的一站式購買，從智能創造培養和訓練到專科保護珍愛，環繞擺設的智能晉級變革、消費加工、擺設保護、本領培養和訓練等多個上面，實行消費的智能、高效、環境保護和安定。不妨接洽（一...

工碩士呆板人本領有限公司動作智能工場效勞商，從智能創造處置計劃的供給，到產業電氣產物的一站式購買，從智能創造培養和訓練到專科保護珍愛，環繞擺設的智能晉級變革、消費加工、擺設保護、本領培養和訓練等多個上面，實行消費的智能、高效、環境保護和安定。不妨接洽（一九九，二一二七，二六六五）

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呆板人