SMC_TrafoF_ArticulatedRobot_6DOF (FB)

FUNCTION_BLOCK SMC_TrafoF_ArticulatedRobot_6DOF

SMC_TrafoF_ArticulatedRobot_6DOF implements the mathematic forward transformation of an articulated robot with six rotary axes used for determinating position and orientation of the end effector with the axis angles.

Note

All drives are assumed to be scaeld so that one technical unit [u] corresponds to one degree.

InOut:

Scope	Name	Type	Comment
Inout	config	SMC_TrafoConfig_ArticulatedRobot_6DOF	The configuration settings of the robot, SMC_TrafoConfig_ArticulatedRobot_6DOF
	Drive0	AXIS_REF_SM3	The drive of axis 0, AXIS_REF_SM3
	Drive1	AXIS_REF_SM3	The drive of axis 1
	Drive2	AXIS_REF_SM3	The drive of axis 2
	Drive3	AXIS_REF_SM3	The drive of axis 3
	Drive4	AXIS_REF_SM3	The drive of axis 4
	Drive5	AXIS_REF_SM3	The drive of axis 5
Output	vToolPos	SMC_Vector3D	Current position of the tool center point in world coordinates
	mToolOrientation	SMC_Matrix3	Tool orientation as a rotation matrix
	armState	SMC_ArticulatedRobot_6DOF_J0_State	State of joint 0
	elbowState	SMC_ArticulatedRobot_6DOF_J1_State	State of joint 1
	handState	SMC_ArticulatedRobot_6DOF_J4_State	State of joint 4