Kin_4AxesPalletizer (FB)

 

FUNCTION_BLOCK Kin_4AxesPalletizer IMPLEMENTS ISMKine­maticWithConfigurations,

Transformation FB for a 4 axes palletizer.

The 4-axis palletizer is a common robot type, employed mostly for pallet­izing tasks. It has 4 controlled rotary axes (drawn in red in the figure aside) and a fifth rotary mechanical axis (drawn in grey). The fifth axis mechanically keeps the gripper horizontally aligned.

4AxesPalletizer.png

The coordinate system at the bottom of the robot is the machine coordi­nate system. The Z axis points vertically upwards, the X axis points "forward", i.e. that direction the robot arm points to if it is in zero position. The coordinate system origin is the intersection point of the joint axis 1 and the bottom side of the robot. The picture aside shows the rotation direction of the 4 axes. The black arrows run along the joint axis. The direction of rotation is defined according to the "right-hand rule": If the thumb of the right hand is along with the arrow, then the positive direction of rotation is along with the slightly curved fingers of the hand. For example, seen from the top, the positive direction of rotation of axis 0 is counterclockwise, axis 1 and axis 2 tilt forward.

4AxesPalletizer_2.png

The picture aside shows the kinematics in zero position of all axes: The names of the parameters are according to the Denavit-Hartenberg convention.

4AxesPalletizer_3.png

Robots with the following Denavit-Hartenberg configuration are supported (the values of d_i and a_i are supplied in the configuration):

joint number

joint offset

link offset d_i

link length a_i

link twist

0

d1

a1

-90°

1

-90°

0

a2

2

90°

0

a3

3

0

a4

90°

4

d5

0

180°

In contrast to our convention, the Z axis of the tool coordinate system points in the direction of the tool, that is, away from the robot.

The single axes values have the following interpretation:

a0

position of the first rotary axis around Z in degrees

a1

position of the second rotary axis in degrees

a2

position of the third rotary axis in degrees

a3

position of the fourth rotary axis in negative direction of Z axis in degrees

The axes have the following maximum ranges:

  • Axis 0: ]-180°; 180°[

  • Axis 1: [-90°; 90°]

  • Axis 2: [-180°; 90[

  • Axis 3: not limited, the range can be larger than 360°

Note

This position kinematics does not support orientation mode "Axis" for CP movements.

Attributes:

sm_kin_libdoc

InOut:

Scope

Name

Type

Comment

Input

d1

LREAL

Denavit-Hartenberg Parameter d1, >= 0

a1

LREAL

Denavit-Hartenberg Parameter a1, >= 0

a2

LREAL

Denavit-Hartenberg Parameter a2, > 0

a3

LREAL

Denavit-Hartenberg Parameter a3, > 0

a4

LREAL

Denavit-Hartenberg Parameter a4, >= 0

d5

LREAL

Denavit-Hartenberg Parameter d5, < 0