Delta Tau GEO BRICK LV Manual Do Utilizador
Turbo PMAC User Manual
266
Setting Up a Coordinate System
Turbo PMAC will also set Q10 to 1 in this mode as a flag to the inverse-kinematic program that it should
use these axis (tip) velocity values to compute motor (joint) velocity values.
use these axis (tip) velocity values to compute motor (joint) velocity values.
In this mode, after any execution of the inverse-kinematic program, Turbo PMAC will read the values in
those variables P1xx (P101 – P132) for each Motor xx in the coordinate system defined as inverse-
kinematic axes (#xx->I). These are floating-point values, and Turbo PMAC expects to find them in
units of counts per Isx90 milliseconds. Turbo PMAC will use them as motor (joint) velocity values along
with the position values in Pxx to create a PVT move for the motor.
those variables P1xx (P101 – P132) for each Motor xx in the coordinate system defined as inverse-
kinematic axes (#xx->I). These are floating-point values, and Turbo PMAC expects to find them in
units of counts per Isx90 milliseconds. Turbo PMAC will use them as motor (joint) velocity values along
with the position values in Pxx to create a PVT move for the motor.
For PVT moves, then, the inverse-kinematic program not only must take the axis (tip) position values in
Q1 – Q9 and convert them to motor (joint) position values in P1 – P32; but also it must take the axis (tip)
velocity values in Q11 – Q19 and convert them to motor (joint) velocity values in P101 – P132.
Technically, the velocity conversion consists of the solution of the “inverse Jacobian matrix” for the
mechanism.
Q1 – Q9 and convert them to motor (joint) position values in P1 – P32; but also it must take the axis (tip)
velocity values in Q11 – Q19 and convert them to motor (joint) velocity values in P101 – P132.
Technically, the velocity conversion consists of the solution of the “inverse Jacobian matrix” for the
mechanism.
Example
Continuing with the “shoulder-elbow” robot of the above examples, the equations for joint velocities as a
function of tip velocities are:
Continuing with the “shoulder-elbow” robot of the above examples, the equations for joint velocities as a
function of tip velocities are:
[
] [
]
B
sin
2
L
1
L
Y
Y
X
X
B
sin
2
L
1
L
Y
)
B
A
sin(
2
L
A
sin
1
L
X
)
B
A
cos(
2
L
A
cos
1
L
B
B
sin
2
L
1
L
Y
)
B
A
sin(
2
L
X
)
B
A
cos(
2
L
A
&
&
&
&
&
&
&
&
−
−
=
+
−
−
+
+
−
−
=
+
+
+
=
The angles A and B have been computed in the position portion of the inverse-kinematic program. Note
that the velocities become infinite as the angle B approaches 0 degrees or 180 degrees. Since in our
example we are limiting ourselves to positive values for B, we will trap any solution with a value of B less
than 1
that the velocities become infinite as the angle B approaches 0 degrees or 180 degrees. Since in our
example we are limiting ourselves to positive values for B, we will trap any solution with a value of B less
than 1
o
or greater than 179
o
(sin B < 0.0175) as an error.
&1
OPEN INVERSE
CLEAR
{Position calculations from above}
IF (Q10=1)
; PVT mode?
Q26=SIN(Q25)
; sin(B)
IF (Q26>0.0175)
; Not near singularity?
Q27=Q91*Q92*Q26
; L1*L2*sinB
Q28=COS(Q25+Q22)
; cos(A+B)
Q29=SIN(Q25+Q22)
; sin(A+B)
Q30=(Q92*Q28*Q17+Q92*Q29*Q18)/Q27 ; dA/dt
Q31=(-Q7*Q17-Q8*Q18)/Q27
; dB/dt
P101=Q30*Q93
; #1 speed in cts/(Isx90 msec)
P102=Q31*Q93
; #2 speed in cts/(Isx90 msec)
ELSE
; Near singularity
M5182=1
; Set run-time error bit
ENDIF
ENDIF
CLOSE
Note that in this case the check to see if B is near 0
o
or 180
o
is redundant because we have already done
this check in the position portion of the inverse-kinematic algorithm. This check is shown here to illustrate
the principle of the method. In this example, a run-time error is created if too near a singularity; other
strategies are possible.
the principle of the method. In this example, a run-time error is created if too near a singularity; other
strategies are possible.