SMC_Trafo_ArticulatedRobot_6DOF (FB)
FUNCTION_BLOCK SMC_Trafo_ArticulatedRobot_6DOF
SMC_Trafo_ArticulatedRobot_6DOF implements the mathematic inverse transformation of an articulated robot with six rotary axes used for calculation of the tool center point coordinates from the axes position, the tool orientation and the states of joint.
Robots with the following denavit-hartenberg configuration are supported: (the values of d_i and a_i are supplied in the configuration)
|
x |
joint offset |
link offset d_i |
link length a_i |
link twist |
|
1 |
0° |
d1 |
a1 |
-90° |
|
2 |
90° |
0 |
a2 |
0° |
|
3 |
0° |
d3 |
a3 |
90° |
|
4 |
0° |
d4 |
0 |
90° |
|
5 |
0° |
0 |
0 |
-90° |
|
6 |
0° |
d6 |
0 |
0° |
Assumptions:
All lengths in config are positive
(greater than g_fSMC_CNC_EPS)
The valid range for axis 0, 1, 3, and 4
is contained in ]-180°, 180°[
The valid range for axis 2 (dQ2)
is contained in ]-90°, 180°[
The valid range for axis 5 (dQ5)
is not constrained and may be greater than 360°
InOut:
|
Scope |
Name |
Type |
Comment |
|
Inout |
config |
The configuration settings of the robot SMC_TrafoConfig_ArticulatedRobot_6DOF |
|
|
pi |
Position and orientation in world cooridinates: dX, dY, dZ, dA (yaw), dB (pitch), dC (roll) SMC_POSINFO |
||
|
Input |
dQ0_old |
LREAL |
The old value of axis 0 in degrees |
|
dQ1_old |
LREAL |
The old value of axis 1 in degrees |
|
|
dQ2_old |
LREAL |
The old value of axis 2 in degrees |
|
|
dQ3_old |
LREAL |
The old value of axis 3 in degrees |
|
|
dQ4_old |
LREAL |
The old value of axis 4 in degrees |
|
|
dQ5_old |
LREAL |
The old value of axis 5 in degrees |
|
|
armState |
State of joint 0, (forward, straight, backwards) |
||
|
elbowState |
State of joint 1, (up, straight, down) |
||
|
handState |
State of joint 4, (up, straight, down) |
||
|
Output |
dQ0 |
LREAL |
The value of axis 0 in degrees |
|
dQ1 |
LREAL |
The value of axis 1 in degrees |
|
|
dQ2 |
LREAL |
The value of axis 2 in degrees |
|
|
dQ3 |
LREAL |
The value of axis 3 in degrees |
|
|
dQ4 |
LREAL |
The value of axis 4 in degrees |
|
|
dQ5 |
LREAL |
The value of axis 5 in degrees |
|
|
bError |
BOOL |
If TRUE, an error has occured during computation |
|
|
result |
Result of the computation in detail |
