Delta Tau GEO BRICK LV User Manual
Turbo PMAC User Manual
262
Setting Up a Coordinate System
The forward-kinematic program must calculate the axis positions for all of the axes in the coordinate
system, whether or not all of the motor positions are calculated in the inverse-kinematic program (see
below). For instance, if this arm had a vertical axis at the tip with a normal axis definition statement in
C.S. 1 of #3->100Z (100 counts per millimeter – a linear relationship between motor and axis), the above
program would still need to perform the forward-kinematic calculation for this motor/axis with a line such
as Q9=P3/100.
system, whether or not all of the motor positions are calculated in the inverse-kinematic program (see
below). For instance, if this arm had a vertical axis at the tip with a normal axis definition statement in
C.S. 1 of #3->100Z (100 counts per millimeter – a linear relationship between motor and axis), the above
program would still need to perform the forward-kinematic calculation for this motor/axis with a line such
as Q9=P3/100.
Note:
If the forward-kinematic algorithm is not correct, and does not yield a true
mathematical inverse of the inverse-kinematic algorithm, there will be a sudden and
potentially dangerous jump at the beginning of the first move executed after the
forward kinematic algorithm is executed. Make sure early in development that the
Ixx11 fatal following error limits are set as tight as possible to ensure that any large
errors will cause a trip and not result in violent motion.
mathematical inverse of the inverse-kinematic algorithm, there will be a sudden and
potentially dangerous jump at the beginning of the first move executed after the
forward kinematic algorithm is executed. Make sure early in development that the
Ixx11 fatal following error limits are set as tight as possible to ensure that any large
errors will cause a trip and not result in violent motion.
Iterative Solutions
Some systems, particularly parallel-link mechanisms such as Stewart platforms (hexapods), do not have
reasonable closed-form solutions for the forward-kinematic equations, and require iterative numerical
solutions. Typically, these cases are handled by a looping WHILE … ENDWHILE construct in the
forward-kinematic program. Do not permit indefinite looping – if the solution does not converge in the
expected number of cycles, the program should be stopped (see the inverse-kinematic equations, below,
for examples of how to stop the program).
Some systems, particularly parallel-link mechanisms such as Stewart platforms (hexapods), do not have
reasonable closed-form solutions for the forward-kinematic equations, and require iterative numerical
solutions. Typically, these cases are handled by a looping WHILE … ENDWHILE construct in the
forward-kinematic program. Do not permit indefinite looping – if the solution does not converge in the
expected number of cycles, the program should be stopped (see the inverse-kinematic equations, below,
for examples of how to stop the program).
In this case, it is best to leave the I11 program-calculation delay variable at its default value of 0, so the
calculations can take as long as needed. If I11 is greater than 0, and the forward-kinematic calculations
plus the first move calculations do not finish within I11 msec, Turbo PMAC will stop the program with a
run-time error. In any case, if the forward-kinematic calculations take more than about 25 msec, it is
possible to trip the watchdog timer.
calculations can take as long as needed. If I11 is greater than 0, and the forward-kinematic calculations
plus the first move calculations do not finish within I11 msec, Turbo PMAC will stop the program with a
run-time error. In any case, if the forward-kinematic calculations take more than about 25 msec, it is
possible to trip the watchdog timer.
Position-Reporting Forward Kinematics
Another use of forward-kinematic calculations is for the position reporting function, reading actual joint
positions at any time, and converting them to tip positions for reporting. The forward-kinematic program
buffer on Turbo PMAC does not support this function. (Using the program for both initial-position
calculations and position reporting could lead to potential overlapping use and register conflicts.)
positions at any time, and converting them to tip positions for reporting. The forward-kinematic program
buffer on Turbo PMAC does not support this function. (Using the program for both initial-position
calculations and position reporting could lead to potential overlapping use and register conflicts.)
If the application requires the Turbo PMAC to do forward-kinematic calculations for position reporting as
well as for establishing initial tip position, the position-reporting calculations should be put into a PLC
program. The following PLC program could be used for the position-reporting function of the example
“shoulder-elbow” robot:
well as for establishing initial tip position, the position-reporting calculations should be put into a PLC
program. The following PLC program could be used for the position-reporting function of the example
“shoulder-elbow” robot:
; M-variable definitions for actual position registers
M162->D:$8B
; Motor 1 actual position
M262->D:$10B
; Motor 2 actual position
; Forward-kinematic PLC program buffer for position reporting
OPEN PLC 10
; Forward kinematics for CS 1
CLEAR
; Erase existing contents
P51=M162/(I108*32*Q93)
; Actual A position (deg)
P52=M262/(I208*32*Q93)
; Actual B position (deg)
Q27=Q91*COS(P51)+Q92*COS(P51+P52)
; Actual X position
Q28=Q91*SIN(P51)+Q92*SIN(P51+P52)
; Actual Y position
CLOSE