Y_MA_ConstructFrameMatrix

Function block to convert a cartesian position orientation vector (X, Y, Z, Rx, Ry, Rz) and converts it to a position orientation 4 x 4 matrix that can be readily used for transformation calculations.

Parameters:

*	Parameter	Data Type	Description	Default
VAR_IN_OUT
B	Position	VECTOR	First 6 values are used [X Y Z Rx Ry Rz]. Rx Ry Rz are assumed to be in degrees.
B	Frame	Yt_MA_Matrix4x4	16 values are used to represent a 4x4 matrix.
VAR_INPUT
B	Enable	BOOL	The function block will continue to execute every scan while Enable is TRUE and there are no errors.	FALSE
VAR_OUTPUT
B	Valid	BOOL	The Valid output indicates that the function block is operating normally and the outputs of the function block are valid.

Notes:

OUTPUT: Frame [Matrix_4x4] -- Array of 16 values are used to represent a 4x4 matrix. [a11 a12 a13 a14 a21 a22 a23 a24 a31 a32 a33 a34 a41 a42 a43 a44]

Frame matrix is the form:

T = [nx ox ax px; ny oy ay py; nz oz az pz; 0 0 0 1 ]

p describes the location of the frame origin

n describes unit vector of x-axis of the frame

o describes unit vector of y-axis of the frame

a describes unit vector of z-axis of the frame

In terms of pose vector [X Y Z Rx Ry Rz]:

C() = cos(), S() = sin()


T	=	C(Rz)C(Ry)	C(Rz)S(Ry)S(Rx)-S(Rz)C(Rx)	C(Rz)S(Ry)C(Rx)+S(Rz)S(Rx)	X
		S(Rz)C(Ry)	S(Rz)S(Ry)S(Rx)+C(Rz)C(Rx)	S(Rz)S(Ry)C(Rz)-C(Rz)S(Rx)	Y
		-S(Ry)	C(Ry)S(Rx)	C(Ry)C(Rx)	Z
		0	0	0	1

INPUT: Position [VECTOR] -- First 6 values are used [X Y Z Rx Ry Rz]. Rx Ry Rz are assumed to be in degrees.