Help simultus

ROBOT_EDU_INV_KIN (184)

ID 184

Application:

PLC

SIM

Group: DEVICES

Short description:

Function that calculates the inverse kinematics of an educational robot.

Inputs (31):

no.	type	impulse	mark	notes
1	R	no	R11	[1,1] element of the coordinate system rotation matrix
2	R	no	R12	[1,2] element of the coordinate system rotation matrix
3	R	no	R13	[1,3] element of the coordinate system rotation matrix
4	R	no	R21	[2,1] element of the coordinate system rotation matrix
5	R	no	R22	[2,2] element of the coordinate system rotation matrix
6	R	no	R23	[2,3] element of the coordinate system rotation matrix
7	R	no	R31	[3,1] element of the coordinate system rotation matrix
8	R	no	R32	[3,2] element of the coordinate system rotation matrix
9	R	no	R33	[3,3] element of the coordinate system rotation matrix
10	R	no	x	Position in the axis 'x0' [mm] of the x6 point in relation to the coordinate system of the robot base
11	R	no	y	Position in the axis 'y0' [mm] of the x6 point in relation to the coordinate system of the robot base
12	R	no	z	Position in the axis 'z0' [mm] of the x6 point in relation to the coordinate system of the robot base
13	R	no	Q01	Current joint coordinate of joint no 1
14	R	no	Q02	Current joint coordinate of joint no 2
15	R	no	Q03	Current joint coordinate of joint no 3
16	R	no	Q04	Current joint coordinate of joint no 4
17	R	no	Q05	Current joint coordinate of joint no 5
18	R	no	Q06	Current joint coordinate of joint no 6
19	R	no	min1	Lower limit of the joint coordinate no 1
20	R	no	max1	Upper limit of the joint coordinate no 1
21	R	no	min2	Lower limit of the joint coordinate no 2
22	R	no	max2	Upper limit of the joint coordinate no 2
23	R	no	min3	Lower limit of the joint coordinate no 3
24	R	no	max3	Upper limit of the joint coordinate no 3
25	R	no	min4	Lower limit of the joint coordinate no 4
26	R	no	max4	Upper limit of the joint coordinate no 4
27	R	no	min5	Lower limit of the joint coordinate no 5
28	R	no	max5	Upper limit of the joint coordinate no 5
29	R	no	min6	Lower limit of the joint coordinate no 6
30	R	no	max6	Upper limit of the joint coordinate no 6

Outputs (6):

no.	type	impulse	mark	notes
1	B	nie	error	error
2	I	nie	err_ID	error ID
3	R	nie	theta1	Joint coordinate no 1
4	R	nie	theta2	Joint coordinate no 2
5	R	nie	theta3	Joint coordinate no 3
6	R	nie	theta4	Joint coordinate no 4
7	R	nie	theta5	Joint coordinate no 5
8	R	nie	theta6	Joint coordinate no 6

Settings:

no.	type	mark	notes
1	R	d1	Length [mm] of the first segment; default - 1
2	R	a2	Length [mm] of the second segment; default - 1
3	R	d4	Length [mm] of the third segment; default - 1
4	R	d6	Length [mm] of the fourth segment; default - 1

Operation:

The ROBOT_EDU_INV_KIN function calculates the inverse kinematics of an educational robot, i.e. the joint coordinates of the robot based on the requested position (x, y, z) and orientation (R11 ... R33) in relation to the coordinate system related to the base. The right-hand coordinate system has been adopted. The joints are marked: J1, J2, J3, J4, J5, J6. Information about the current joint coordinates is transferred to the Q01 ... Q06 inputs, so that in the next time step the robot adopts the configuration closest to the current one (there are several solutions to joint coordinates for one position). At the min and max inputs, the minimum and maximum values for the joint coordinates are set. These are most often the limitations resulting from the construction of the robot. The error output informs about the current state of the performed calculations. The table with the error codes and the inverse kinematics algorithm are below.

error ID	notes
0	no error
1	position obtained, orientation impossible
2	position impossible to obtain (outside the working space)
3	singular position
-1	calculation error (incorrect input data)

The algorithm of solving the inverse problem - PROF robots

Fig. Direction of rotation

Fig. Coordinate systems

R registers and M markers used: none