Stage Robot Kinematic Module
General Kinematic Description
This robot module controls a 3- or 4-degree-of-freedom (DOF) high-precision stage robot. The basic geometry of this robot is identical to a XYZTheta gantry robot, except that the final axis points up along the positive world-Z axis rather than down. This module was designed to control the movement of a stage, table or other similar device on which a work piece is typically mounted.
The first three axes of the robot consist of three orthogonal linear axes that move in the X, Y, and Z directions. The optional fourth logical axis is a rotation about the world Z axis (Theta). Normally, the Theta axis is actuated with a rotary drive. However, this axis can be optionally rotated by a linear drive that attaches to a radial arm. Independent of whether the Theta axis is actuated by a rotary or linear drive, its motion is always logically considered to be a rotary motion from the perspective of the operator.
In order to allow a high-precision device to be controlled, this module includes split-axis control on the X axis plus special features to compensate for non-orthogonality between the X and Y axes, and an offset between the encoders for the split axes.
Module Specifications
Minimum V+ Compatibility: V+ 16.1A10
Device Module File Name: STG
Device Module Identification Number: 41
Default Startup Message: "Enhanced Stage Robot Module."
Default Joint Configuration and Mapping:
Joint – Axis
Motor
Servo Board
Board Channel
1 – X
1
1
1
2 – Y
2
1
2
3 – Z
3
1
3
4 – Theta
4
1
4
1 – W Split axis
0
0
0
Additional License requirements: "Enhanced Kinematics License"
If you install this module but do not have the above license, the message "*Option not installed*" will be displayed when you restart your system.
Robot Option Word
NOTE: Bit numbers start with bit #1.
Bit
Default
Description
1
Off
If on, split-axis control for the X axis is enabled. X-axis split-axis control means that the first and last motors are coupled and move as a pair to actuate the X axis. In this case, the last axis is referred to as W (or X’).
6
Off
If on, the Theta axis is driven by a linear actuator rather than a rotary drive.
Robot Model and Robot Serial Number, Default: 0, 0
Specific Link Descriptions
This robot module controls a mechanism with three or four degrees of freedom. The first three degrees of freedom (DOF) are prismatic joints that position the center of the tool flange. The fourth DOF is a rotary joint that rotates the tool flange about the tool-Z axis. The exact descriptions for these degrees of freedom are as follows:
Joint
Description
1
Joint 1 is a linear axis that moves in the world-X direction. A positive displacement of the joint moves the robot end effector in the positive world-X direction.
2
Joint 2 is a linear axis that moves in the world-Y direction. A positive displacement of the joint moves the robot end effector in the positive world-Y direction.
3
Joint 3 is a linear axis that moves in the world-Z direction. A positive displacement of the joint moves the robot end effector in the positive world-Z direction.
4
Joint 4 is an optional revolute (theta) axis about the world-Z direction. A positive rotation of the joint turns the robot end effector in a positive direction relative to the world-Z axis. That is, if a NULL tool is defined, the Z axis of the tool frame will be collinear with the axis of rotation of joint 4, and will be pointed in the direction of the positive world-Z axis. When this joint is at its zero position, the tool-Z axis points along +world Z, tool X points along +world X, and tool Y points along the +world Y direction.
As with other robot modules, the standard V+ BASE and TOOL transformations can be used in combination with the geometric model to specify and compute the end point of the robot relative to the world coordinate frame.
Variations in Axis Configuration
This module can be configured to control either a three-degree-of-freedom (X/Y/Z) system or a four-degree-of-freedom (X/Y/Z/Theta) system. In addition, as described above, the X axis can be configured for single- or split-axis control and the Theta axis can be driven by a rotary or linear actuator.
Link Dimensions (Geometric Dimensional Constants)
In addition to the standard tool-Z offset, this kinematic module has several parameters that specify the geometric parameters of the controlled robot. These parameters are defined as follows:
#
Parameter
Description
1
Alpha
This is the "yaw" angle. It specifies (in degrees) the correction necessary to compensate for the non-perpendicularity between the X and the Y joints. The interpretation of this angle is illustrated in Figure 1. When this value is set to 0 degrees, the X and Y joints are perfectly perpendicular.
2
X-W Offset
If split-axis control is enabled, this is an encoder offset that is added to the (split) W axis to account for different zero positions between the X and W encoders. That is, when the split axes are not being servoed to a position and the Y and X axes have a yaw angle of Alpha, the difference between the W and X encoder readings is equal to the X-W Offset.
3
X Encoder Offset
This parameter allows the zero for the X motor encoder to be set arbitrarily relative to the zero position of the X joint. When the X motor encoder value is read, this offset in encoder counts is subtracted prior to conversion to world coordinate units.
4
Y Encoder Offset
Offset for Y motor encoder in encoder counts.
5
Z Encoder Offset
Offset for Z motor encoder in encoder counts.
6
Theta Encoder Offset
Offset for Theta motor encoder in encoder counts.
7
W Encoder Offset
Offset for W motor encoder in encoder counts.
8
Theta Radial Arm
If the Theta axis is driven by a linear actuator that pushes and pulls on a radial lever arm, this is the radial length of the level arm.
9
X Hall Sensor
Calibration offset for X motor Hall Effect sensor
10
Y Hall Sensor
Calibration offset for Y motor Hall Effect sensor
11
Z Hall Sensor
Calibration offset for Z motor Hall Effect sensor
12
Theta Hall Sensor
Calibration offset for Theta motor Hall Effect sensor
13
W Hall Sensor
Calibration offset for W motor Hall Effect sensor
Interpretation of Cartesian Rotations
During program-generated straight-line motions, the first Cartesian rotation speed controls the rate at which joint 4 rotates, and the speed should be set to be consistent with the joint-interpolated speed for joint 4. No other Cartesian rotational speeds are applicable.
Coupling Between Robot Joints and Motors
As previously described, this module has special compensation for non-orthogonality between the X and Y axes, and encoder zero offsets in the split-axis mode. These compensations are controlled by the link dimension constants "Alpha" and "X-X’ Offset" described above.
Robot Configuration Control Program Instructions
The following robot configuration-control program instructions do not have any effect upon the operation of mechanisms controlled by this module:
ABOVE, BELOW, FLIP, NOFLIP, LEFTY, RIGHTY
Axis Free Mode
Any axis may be placed into a kind of "free mode" where the position servo is disabled and a zero current value is written to the motor. This mode is not exactly the same as MCP free mode, in that no gravity compensation or clutch control is allowed. The mode is controlled by ROBOT.OPR, function_code 0. See the table below. This special free mode is never disabled except by ROBOT.OPR. The user must disable free mode for normal power control to resume for this motor.
Additional Restrictions & Notes
As a special feature, the link parameters ("Alpha", "X-W Offset", etc.) can be dynamically and temporarily modified at runtime via the ROBOT.OPR program instruction. If these values are changed, they take effect immediately. In order to save these values as the permanent values, SPEC must be executed to save the link dimensions. Otherwise, when the system is rebooted, the original SPEC values are reloaded.
ROBOT.OPR is a general-purpose instruction, whose interpretation varies from one robot module to another. The general syntax for this instruction is:
ROBOT.OPR (function_code) exp1, exp2, ..., expn
To change the link dimension values, the following parameters must be specified:
To Change
function_code
exp1
exp2
Axis Free Mode
0
Number of the axis to free or unfree, from 1 to 4.
1 to enter free mode,
0 to exit free mode.
Alpha
1
Temporary value of Alpha, specified in degrees.
N/A
X-W Offset
2
Temporary value of the encoder offset, specified in encoder counts.
N/A
Encoder Offsets
3-7
The encoder offsets for motors X, Y, Z, Theta, W respectively.
N/A
Theta Radial Arm
8
Length of the Theta radial arm in mm.
N/A
Hall Effect Sensor Offsets
9-13
Offsets used by the calibration software to relate the Hall Effect sensor readings to the motor commutation references.
As a convenience, the ROBOT.OPR real-valued functions can be utilized to read the current setting for Alpha, the X-W Offset, or any of the Encoder Offset values. The general syntax for this function is as follows:
value = ROBOT.OPR(2,index)
where "index" corresponds to the function code for the desired parameter and ranges from 1-8.
The following point should be kept in mind when using ROBOT.OPR to change the link dimension values:
This instruction breaks any executing continuous-path motion and modifies the link dimensions after the robot has come to a stop.
View Looking Down At The Stage
Xj, Yj: Positions of the X and Y linear joints measured along the direction of travel of the joints. Xc, Yc: Cartesian position of the stage in the world coordinate system relative to (0,0) a : Yaw angle of the Y linear joint relative to the world Y axis in units of degrees.
Figure 1: Definition of Alpha Yaw Angle
