Kin_TGantry2.GetKinematicsName (METH) ¶ METHOD GetKinematicsName : STRING(255) InOut: Scope Name Type Return GetKinematicsName STRING(255)
MC_KIN_REF_SM3 (ITF) ¶ INTERFACE MC_KIN_REF_SM3 EXTENDS __SYSTEM.IQueryInterface Represents a kinematic transformation that can transform from position/orientation of the TCP to axis positions and vice versa. Note When implementing a new transformation, it is recommended to also implement the following interfaces: ISMKinematicWithConfigurations4 If the kinematics has multiple configurations or modes, such as the “elbowRight” mode of the scara transformations. ISMKinematicWithInfo2 If the kinematics contains rotary joints or wants to restrict the range of a joint. ISMKinematicWithInitialization If the kinematics has to be initialized before using it. ISMPositionKinematics_Offset2 If the kinematics can be combined with orientation kinematics (Required for the orientation interpolation in axis space.) ISMOrientationKinematics3 If it is an orientation kinematics that can be combined with position kinematics. ISMKinPeriodHandling If the kinematics uses rotary axes and allows to configure the period of the rotary axes in the configuration data. ISMKinematicsWithOrientationImage2 This interface allows to calculate the flange orientation given the programmed orientation of the flange. (There can be a difference between programmed and actual orientation if the kinematics does not have all three degrees of freedom for the orientation.) You don’t need to implement this interface for a position or orientation kinematics. ISMOrientationKinematicsWithOriImage2 This interface is required to define the orientation image, that is, the space of possible orientations, of the orientation kinematics. This interface is required by the coupled kinematics to calculate the flange orientation from the programmed orientation. If, instead of ISMPositionKinematics2 / ISMOrientationKinematics3 , only ISMPositionKinematics / ISMOrientationKinematics are implemented, “impossible” orientations may not be handled correctly. If the kinematic transformation needs the limits of an axis for the computation, then the interface ISMKinematicAxisSettings should be implemented. This is usually not needed but can be necessary to return sensible values when inside a singularity. Attention: Once connected to an axis group, the inputs of the function block implementing MC_KIN_REF_SM3 must not change! It is strongly recommended to use a second instance or to encode parametrization in the configuration. Properties: NumAxes Methods: AxesToCartesian CartesianToAxes Structure: AxesToCartesian (Method) CartesianToAxes (Method) NumAxes (Property)
MC_KIN_REF_SM3.AxesToCartesian (METH) ¶ METHOD AxesToCartesian : SMC_Error Computes position/orientation of the TCP from axis positions. Returns whether the computation was successful. InOut: Scope Name Type Comment Return AxesToCartesian SMC_Error Inout f SMC_Frame Out: The position and orientation of the TCP cd CONFIGDATA Out: The serialized configuration data Inout Const a AXISPOS_REF In: The position of all axes
MC_KIN_REF_SM3.CartesianToAxes (METH) ¶ METHOD CartesianToAxes : SMC_Error Computes the axis positions from position/orientation of the TCP. Returns whether the computation was successful. InOut: Scope Name Type Comment Return CartesianToAxes SMC_Error Inout a AXISPOS_REF Out: The position of all axes Inout Const f SMC_Frame In: The position and orientation of the TCP aRef AXISPOS_REF In: The reference position of all axes. The reference position is used to resolve ambiguities, for example due to singular positions, modulo axes, or rotary axes with a range > 360°. If the kinematics does not have ambiguities, or if the configuration data suffices to resolve any ambiguities, the reference position is not used. In case of an ambiguity, the transformation should resolve the ambiguity in such a way that the resulting axis positions are as close as possible to aRef. cd CONFIGDATA In: The serialized configuration data. If a kinematics has singularities, the configuration determines in which “mode” the resulting axis position is requested. For example scara kinematics have a singularity if the scara arms point into the same direction. This singularity divides the axis positions into two modes: one where the angle of the elbow joint is in the range ]0°..180°[, and one where it is in the range ]-180°..0°[. The configuration data, which in this case is a single boolean flag “xElbowRight”, is then used to select the desired mode.
MC_KIN_REF_SM3.NumAxes (PROP) ¶ PROPERTY NumAxes : UDINT Returns the number of axes of the kinematics.
Structs ¶ AXISLIMITS (Alias) AXISPOS_REF (Struct) Axis_Properties (Struct) Axis_Settings (Struct) Axis_Type (Enum) CONFIGDATA (Struct) Coupled_Kin_Axis_Map (Struct) Coupled_Kin_Axis_Type (Enum) MCS_OR_FLANGE (Enum) OrientationDOF (Enum) OrientationSpace (Struct)
AXISLIMITS (ALIAS) ¶ TYPE AXISLIMITS : SMRB.|dAXISLIMITS|
AXISPOS_REF (STRUCT) ¶ TYPE AXISPOS_REF : STRUCT Stores the position of each axis used in an axis group. Note depending on the used kinematics, only a prefix of the values is used, e.g. a0, a1, and a2 for a kinematics with three degrees of freedom. InOut: Name Type Comment a0 LREAL Position of axis 0 a1 LREAL Position of axis 1 a2 LREAL Position of axis 2 a3 LREAL Position of axis 3 a4 LREAL Position of axis 4 a5 LREAL Position of axis 5
Axis_Properties (STRUCT) ¶ TYPE Axis_Properties : STRUCT The properties of a kinematic axis. Note that a rotary axis that is assumed to be programmed in angular degrees. InOut: Name Type Comment eType Axis_Type Linear or rotary joint? limits AXISLIMITS The limits of the joint coming from the kinematics. (Note that the axis connected to the joint may also have software limits. The intersection of axis- and joint- limits is used.)
ISMPositionKinematics_Offset.CartesianToAxes_Offset (METH) ¶ METHOD CartesianToAxes_Offset : SMC_Error Computes the axis positions from position of the flange plus an offset vector. The offset vector is given relative to the orientation of the flange. Note in general, the orientation of the flange is unknown when this method is called. This means that the method as it does not consider an additional offset. If vOffset_TCP is zero, this method returns the same result as MC_KIN_REF_SM3.CartesianToAxes Returns whether the computation was successful. InOut: Scope Name Type Comment Return CartesianToAxes_Offset SMC_Error Inout a AXISPOS_REF Out: The position of the position axes Inout Const v_MCS SMC_Vector3D In: The position of the offset TCP, relative to the machine coordinate system. (The TCP is offset by the additional input vOffset_TCP .) vOffset_TCP SMC_Vector3D In: The additional position offset between the TCP of the position kinematics and the offset TCP. This input is expressed in the flange coordinate system of the position kinmatics. aRef AXISPOS_REF In: The reference position of all position axes. The reference position is used to resolve ambiguities, for example due to singular positions, modulo axes, or rotary axes with a range > 360°. If the kinematics does not have ambiguities, or if the configuration data suffices to resolve any ambiguities, the reference position is not used. In case of an ambiguity, the transformation should resolve the ambiguity in such a way that the resulting axis positions are as close as possible to aRef. cd CONFIGDATA In: The serialized configuration data of the offset position kinematics. (As returned by ISMPositionKinematics_Offset2.AxesToConfiguration_Offset .)