Delta Tau GEO BRICK LV ユーザーズマニュアル

 

To implement these equations in a Turbo PMAC inverse-kinematic program for Coordinate System 1 that 
converts the X and Y tip coordinates in millimeters to the shoulder angle in Motor 1 and the elbow angle in 
Motor 2, the following program could be used.  System constants Q91, Q92, and Q93 are the same as for 
the above forward kinematic program. 

; Setup for program 

&1 

#1->I  

 

 

 

; Motor 1 assigned to inverse kinematic axis in CS 1 

#2->I  

 

 

 

; Motor 2 assigned to inverse kinematic axis in CS 1 

M5182->Y:$00203F,22,1 

 

; CS 1 run-time error bit 

; Pre-compute additional system constants 

Q94=Q91*Q91+Q92*Q92 

 

; L1^2 + L2^2 

Q95=2*Q91*Q92 

  ; 

2*L1*L2 

Q96=Q91*Q91-Q92*Q92 

 

; L1^2 – L2^2 

; Inverse-kinematic algorithm to be executed repeatedly 

&1 OPEN INVERSE 

 

 

; Inverse kinematics for CS 1 

CLEAR 

    ; 

Erase 

existing 

contents 

Q20=Q7*Q7+Q8*Q8 

  ; 

X^2+Y^2 

Q21=(Q20-Q94)/Q95 

  ; 

cos(B) 

IF (ABS(Q21)<0.9998) 

 

; Valid solution w/ 1 deg margin? 

  Q22=ACOS(Q21) 

 

 

; B (deg) 

  Q0=Q7 

 

 

 

; X into cos argument for ATAN2 

  Q23=ATAN2(Q8) 

 

 

; A+C = ATAN2(Y,X) 

  Q24=ACOS((Q20+Q96)/(2*Q91*SQRT(Q20))) ; C (deg) 

  Q25=Q23-Q24 

 

 

; A (deg) 

  P1=Q25*Q93 

 

 

; Motor 1 = 1000A 

  P2=Q22*Q93 

 

 

; Motor 2 = 1000B 

ELSE 

    ; 

Not 

valid, 

halt 

operation 

  M5182=1 

 

 

 

; Set run-time error bit 

ENDIF 

CLOSE 

Notes on the example: 

• 

By choosing the positive arc-cosine solutions, we are automatically selecting the right-armed case.  In 
a more general solution, we would have to choose whether the positive or negative is used, based on 
some criterion. 

• 

Increased computational efficiency could be obtained by combining more operations into single 
assignment statements.  Calculations were split out here for clarity’s sake. 

• 

This example does not use the substitution macros permitted by the Executive program to substitute 
meaningful names for variables.  Use of these substitution macros in complex applications is strongly 
encouraged. 

• 

This example stops the program for cases in which no inverse kinematic solution is possible.  It does 
this by setting the “run-time error” status bit for the coordinate system, which causes Turbo PMAC to 
halt motion program execution and issue the Abort command.  Other strategies may be used to cope 
with this problem. 

If this robot had a vertical axis at the tip, the relationship between motor and axis could be defined with a 
normal linear axis-definition statement (e.g. #3->100Z for 100 counts per millimeter), and the motor 
position would be calculated without the special inverse-kinematic program.  Alternately, the motor could 
be defined as an inverse-kinematic axis (#3->I) and the motor position could be calculated in the inverse-
kinematic program (e.g. Q3=Q49*100 to set Motor 3 position from the Z-axis with 100 counts per unit).