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	SMC_TrafoConfig_ArticulatedRobot_6DOF	The configuration settings of the robot SMC_TrafoConfig_ArticulatedRobot_6DOF
Inout	pi	SMC_PosInfo	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	SMC_ArticulatedRobot_6DOF_J0_State	State of joint 0, (forward, straight, backwards)
	elbowState	SMC_ArticulatedRobot_6DOF_J1_State	State of joint 1, (up, straight, down)
	handState	SMC_ArticulatedRobot_6DOF_J4_State	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	SMC_TrafoResult_ArticulatedRobot_6DOF	Result of the computation in detail