Notes on the documentation

This description is only intended for the use of trained specialists in control and automation engineering who are familiar with the applicable national standards.
It is essential that the following notes and explanations are followed when installing and commissioning these components.

The responsible staff must ensure that the application or use of the products described satisfy all the requirements for safety, including all the relevant laws, regulations, guidelines and standards.

Disclaimer

The documentation has been prepared with care. The products described are, however, constantly under development.
For that reason the documentation is not in every case checked for consistency with performance data, standards or other characteristics.
In the event that it contains technical or editorial errors, we retain the right to make alterations at any time and without warning.
No claims for the modification of products that have already been supplied may be made on the basis of the data, diagrams and descriptions in this documentation.

Trademarks

Beckhoff®, TwinCAT®, EtherCAT®, Safety over EtherCAT®, TwinSAFE® and XFC® are registered trademarks of and licensed by Beckhoff Automation GmbH.
Other designations used in this publication may be trademarks whose use by third parties for their own purposes could violate the rights of the owners.

Patent Pending

The TwinCAT Technology is covered, including but not limited to the following patent applications and patents:
EP0851348, US6167425 with corresponding applications or registrations in various other countries.

Copyright

© Beckhoff Automation GmbH & Co. KG, Germany.
The reproduction, distribution and utilization of this document as well as the communication of its contents to others without express authorization are prohibited.
Offenders will be held liable for the payment of damages. All rights reserved in the event of the grant of a patent, utility model or design.


TwinCAT Kinematic Transformation

The kinematics package integrates itself transparently into the existing Motion Control world of TwinCAT: simple insertion of an additional Kinematic channel is suffi cient. The complete parameterisation is done in the TwinCAT System Manager. The package supports different parallel and serial kinematic systems, as used for e.g. pickand-place tasks.
The supplement is based on TwinCAT NC I for interpolating movements and G-Code (DIN 66025). In addition, standard PTP and cam plate applications can be realised. Programming is usually based on the Cartesian coordinate system, with the control system calculating the inverse kinematics for the relevant motor positions. The kinematic system can be selected in the TwinCAT System Manager. The kinematic channel is used to parameterise the type (e.g. delta) and the bar lengths and offsets. Mass and mass inertia values can be specified for dynamic pre-control. The “flying saw” and “cam plate” functions enable synchronisation with conveyor belts for picking or placing workpieces, for example.

Installation Requirements

The supplement TwinCAT Kinematic Transformation requires TwinCAT NCI-Level and at least Version 2.11 R2 (Build >2000). It runs generally on XP and CE devices.

Installation Levels

TwinCAT Kinematic Transformation is devided in 3 different levels. Depending on the required kinematic transformation the level must be selected. At this the highest level includes all sub-levels.

Level 1

Supports the static transformation. This includes a translation and rotation of the co-ordinate system.

Level 2

Supports level 1 and easy (mainly 2D) kinematic transformation like H-Bot and 2D-Parallel-Kinematics

Level 3

Supports level 2 and more complex kinematic transformation like Delta-Robot


Overview Transformations

Transformation Type | Description | Required Level and Version
Transformation Shell | Level 1 Version 2.11.0 Build 3
Static Transformation | Level 1 Version 2.11.0 Build 3
H-Bot Level 2 Version 2.11.0 Build 3

2-D-Type 1 Level 2 Version 2.11.0 Build 3

2-D-Type 2 | Level 2 Version 2.11 Build 6
2-D-Type 3 | Level 2 Version 2.11 Build 6
Delta Type 1 | Level 3 Version 2.11.0 Build 3


Kinematic Transformation

Considering the PLCopen we distinguish two main co-ordinate systems

Axis Co-ordinate System (ACS)

The axes of the ACS are connected to the real motor. So this co-ordinate system represents the positioning of the motor without taking care about the position of the tool center point (TCP)

Machine Co-ordinate System (MCS)

This is by default a cartesian co-ordinate system. Programming of movements are usually done here and therefore the axes X, Y, and Z are used. The backward transformation is calculating the positions for the axes co-ordinate system. The axes in MCS are pure software axes and of type simulation encoder.

All ACS and MCS axes that are used in a kinematic transformation channel must be created in the System Manager. So for example a Delta-Robot will have 3 ACS axes (M1..M3) and 3 MCS axes (X, Y, Z).

Configure Kinematic Transformation Channel

1. Append Channel

To use the supplement TwinCAT Kinematic Transformation a kinematic channel has to be appended to the System Manager configuration.

Select channel type: NC-Channel (for Kinematic Transformation)

2. Append TComObjects

Appending a channel creates an instance of the kinematic group. To run a real kinematic transformation the type must be selected and therefore TComObjects are used. So this objects have to be appended under the group

Browse for TcNcKin.tmc in TwinCAT\IO-Folder.

To start-up for instance the transformation for a delta-robot select

- DeltaKin

- StaticTrafo

- TrafoShell

3. Assign transformation to group

The transformation group has to know what root module has to be called. Therefore the kinematics object ID must be selected. Usually this is the object ID of the transformation shell.


TcNcKinematicTransformation Static Transformation

TS5110 - TS5112 | TwinCAT Kinematic Transformation

[This is preliminary documentation and subject to change.]

Static Transformation

The static transformation supports a translation and rotation.

On that way it is possible to program comfortable in a User Co-ordinate System (UCS) and this module takes care of the transformation to the Machine Co-ordinate System (MCS)

The parameterization is done from MCS to UCS (forward transformation).

First the translation must be described and then the rotation is calculated. Therefore the yaw-pitch-roll-rule that is also described in DIN9300 is used. The calculation sequence for the forward transformation is Z, Y', X''

Parameter | Description
Translation X | Shift in x direction from MCS to user defined co-ordinate system
Translation Y | Shift in y direction from MCS to user defined co-ordinate system
Translation Z | Shift in z direction from MCS to user defined co-ordinate system
Rotation X | Right handed rotation about the x''-axis in degree
Rotation Y | Right handed rotation about the y'-axis in degree
Rotation Z | Right handed rotation about the z-axis in degree
NextTrafoOID | In case of a cascaded static transformation object id of the next transformation is parameterized here


TcNcKinematicTransformation 2-D-Type 1

TS5110 - TS5112 | TwinCAT Kinematic Transformation

2-D-Kinematics Type 1

The 2-D-kinematics type 1 is build up as shown in the schema above.

All motors are scaled in degree and 0° is defined as shown in the schema with positive direction in direction of the arrow.

Parameter for kinematics

Parameter | Description | Unit
InnerArmLength | Length from pivot point to pivot point of the inner arm. That is the arm which is directly linked to the motor | mm
OuterArmLength | Length from pivot point to pivot point of the outer arm. | mm
Displacement | Length from center of the ground plate to the virtual rotary axes of the inner arm | mm

Parameter for dynamic modell

Parameter | Description | Unit
InnerArmMass | Total mass of the inner arm | kg
InnerArmMomentOfInertia | Moment of inertia of the inner arm related to the turning point P1 that is connected to the motor | kg mm^2
OuterArmMass | Mass of the outer arm. Optionally the link mass can described in an own parameter | kg
FirstLinkMass Mass of the link that connects inner and outer arm. Can be used if the link mass is not yet included to outer and inner arms.
The link mass that connects the gripper plate and the outer arm is not specified here. That can be added to the TcpMass
First link mass is related to the inner arm that is linked to motor 1 		kg
SecondLinkMass cf. FirstLinkMass
Second link mass is related to the inner arm that is linked to motor 2 		kg
TcpMass | Mass of the tool center point including gripper plate and gripper. The payload is usually written to a separate parameter. | kg
FirstDriveTorqueOID Object ID of the first drive torque
If motors and gear boxes for all motors behaves similar, all drive torques can be represented by one OID. So both parameters refer to the same object ID.
SecondDriveTorqueOID | Object ID of the second drive torque
GravityOrientationOID | Object ID that describes the mounting orientation. This parameter is used if the gripper does not show to the geocenter. If the robot is mounted as shown on the picture this OID can be zero.

Required Product Level

Level 2


TcNcKinematicTransformation 2-D-Type 2

TS5110 - TS5112 | TwinCAT Kinematic Transformation

2-D-Kinematics Type 2

The 2-D-kinematics type 2 is build up as shown in the schema above.

All motors are scaled in degree and 0° is defined as shown in the schema with positive direction in direction of the arrow.

Parameter for kinematics

Parameter | Description | Unit
InnerArmLength | Length from pivot point to pivot point of the inner arm. That is the arm which is directly linked to the motor | mm
OuterArmLength | Length from pivot point to pivot point of the outer arm. | mm
Displacement | Length from center of the ground plate to the virtual rotary axes of the inner arm | mm
TcpLinkDistance | Distance from pivot point to pivot point of the outer arms | mm

Parameter for dynamic modell

Parameter | Description | Unit
InnerArmMass | Total mass of the inner arm | kg
InnerArmMomentOfInertia | Moment of inertia of the inner arm related to the turning point P1 that is connected to the motor | kg mm^2
OuterArmMass | Mass of the outer arm. Optionally the link mass can described in an own parameter | kg
FirstLinkMass Mass of the link that connects inner and outer arm. Can be used if the link mass is not yet included to outer and inner arms.
The link mass that connects the gripper plate and the outer arm is not specified here. That can be added to the TcpMass
First link mass is related to the inner arm that is linked to motor 1 		kg
SecondLinkMass cf. FirstLinkMass
Second link mass is related to the inner arm that is linked to motor 2 		kg
TcpMass | Mass of the tool center point including gripper plate and gripper. The payload is usually written to a separate parameter. | kg
FirstDriveTorqueOID Object ID of the first drive torque
If motors and gear boxes for all motors behaves similar, all drive torques can be represented by one OID. So both parameters refer to the same object ID.
SecondDriveTorqueOID | Object ID of the second drive torque
GravityOrientationOID | Object ID that describes the mounting orientation. This parameter is used if the gripper does not show to the geocenter. If the robot is mounted as shown on the picture this OID can be zero.

Required Product Level

Level 2


TcNcKinematicTransformation 2-D-Type 3

TS5110 - TS5112 | TwinCAT Kinematic Transformation

2-D-Kinematics Type 3

The 2-D-kinematics type 3 is build up as shown in the schema above.

All motors are scaled in degree and 0° is defined as shown in the schema with positive direction in direction of the arrow.

This kinematic type is implemented as left handed. The shaft of motor 1 & 2 are located in the origin of the co-ordinate system.

Parameter for kinematics

Parameter | Description | Unit
InnerArmLength | Length from motor shaft to pivot point of the outer arm. | mm
OuterArmLength | Length from pivot point to tool center point of the outer arm. | mm

Parameter for dynamic modell

Parameter | Description | Unit
InnerArmMass | Total mass of the inner arm | kg
OuterArmMass | Mass of the outer arm. | kg
ToolCenterpointMass | Mass of the tool center point including gripper plate and gripper. The payload is usually written to a separate parameter. | kg
FirstDriveTorqueOID Object ID of the first drive torque
If motors and gear boxes for all motors behaves similar, all drive torques can be represented by one OID. So both parameters refer to the same object ID.
SecondDriveTorqueOID | Object ID of the second drive torque
GravityOrientationOID | Object ID that describes the mounting orientation. This parameter is used if the gripper does not show to the geocenter. If the robot is mounted as shown on the picture this OID can be zero.

Required Product Level

Level 2


TcNcKinematicTransformation Delta Type 1

TS5110 - TS5112 | TwinCAT Kinematic Transformation

Delta Type 1

The delta kinematics type 1 is build up as shown in the schema above. The kinematic transformation expects ball joints (or elements with the same behavior) in the connection of the arms and to the lower plate.

Optionally the center stick for the orientation of the gripper can be parameterized.

All motors are scaled in degree and 0° is defined as shown in the schema with positive direction in direction of the arrow. That is for all 3 motors the same.

Parameter for kinematics

Parameter | Description | Unit
InnerArmLength | Length from pivot point to pivot point of the inner arm. That is the arm which is directly linked to the motor | mm
OuterArmLength | Length from pivot point to pivot point of the outer arm. | mm
Displacement | Length from center of the ground plate to the virtual rotary axes of the inner arm | mm
TcpDisplacement | Length from center of the gripper plate to the virtual rotary axes of the outer arm | mm

Parameter for dynamic modell

Parameter | Description | Unit
InnerArmMass | Total mass of the inner arm | kg
InnerArmMomentOfInertia | Moment of inertia of the inner arm related to the turning point P1 that is connected to the motor | kg mm^2
OuterArmMass | Mass of the outer arm. If there are 2 rods the total mass is required. Optionally the link mass can described in an own parameter | kg
LinkMass Mass of the link that connects inner and outer arm. Can be used if the link mass is not yet included to outer and inner arms.
The link mass that connects the gripper plate and the outer arm is not specified here. That can be added to the TcpMass 		kg
TcpMass | Mass of the tool center point including gripper plate and gripper. The payload is usually written to a separate parameter. | kg
CenterStickMass | Total mass of the center stick | kg
CenterStickMomentOfInertia | Moment of inertia of the center stick related to the center of gravity (P2) | kg mm^2
CenterStickCenterOfMassDisplacement | Length from gripper plate to center of gravity of the stick. | mm
FirstDriveTorqueOID Object ID of the first drive torque
If motors and gear boxes for all motors behaves similar, all drive torques can be represented by one OID. So all 3 parameters refer to the same object ID.
SecondDriveTorqueOID | Object ID of the second drive torque
ThirdDriveTorqueOID | Object ID of the third drive torque
GravityOrientationOID | Object ID that describes the mounting orientation. This parameter is used if the gripper does not show to the geocenter. If the robot is mounted as shown on the picture this OID can be zero.

Required Product Level

Level 3


TcNcKinematicTransformation Drive Torque

TS5110 - TS5112 | TwinCAT Kinematic Transformation

Drive Torque

The drive torque represents the inertia and efficency of motor and gear box. This is used for a precise calculation of the dynamic model.

Parameter for drive

Parameter | Description | Unit
MomentOfInertia | Rotor Moment of inertia of the motor | kg mm^2

Parameter for gear box

Parameter | Description | Unit
Ratio | Radio of gear box
MomentOfInertia | Moment of inertia of the gear box related to the drive | kg mm^2

Required Product Level

Level 1


TcNcKinematicTansformation.lib Overview

TS5110 - TS5112 | TwinCAT Kinematic Transformation

Overview

[This is preliminary documentation and subject to change.]

Development Environment | Target System | PLC libraries to include
TwinCAT v2.11.2000 | PC(i386) | TcNcKinematicTransformation.lib
Function block | Description
FB_CheckActualStatus | Gets the state of the kinematic transformation group
FB_ConfigKinGroup | Configures ACS and MCS axes to the kinematic transformation group
FB_ResetKinGroup | Resets the kinematic transformation group
FB_UnlockTrafoParam | Paramterises a static trafo as a coordinate system from axis positions
FB_CalcTrafo | Calculates the kinematic transformation
FB_CalcMultiTrafo | Calculates the kinematic transformation for multiple poses
FB_LockTrafoParam | Locks the kinematic transformation group parameters, disables write access
FB_UnlockTrafoParam | Unlocks the kinematic transformation group parameters, enable write access
E_KINSTATUS | Gets the kinematic group configuration
E_KINSTATUS | Gets the kinematic group status
E_KINSTATUS | Enum status of Kinematic Group
ST_KinAxes | Structure of ACS and MCS positions


FB_CheckActualKinStatus

Interface

VAR_INPUT
    bExecute  :    BOOL;
END_VAR

VAR_IN_OUT
   stAxesListReference  :   ST_KinAxes;
   stKinRefIn           :   NciChannelToPlc;
END_VAR

VAR_OUTPUT
   eKinStatus    :   E_KINSTATUS;
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

TYPE E_KINSTATUS :
(
   KinStatus_Error,
   KinStatus_Empty,
   KinStatus_Unknown,
   KinStatus_StartPending,
   KinStatus_Ready
);
END_TYPE

Description

The Function Block FB_CheckActualKinStatus outputs the status of the Kinematic channel.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

VAR
   fbFB_CheckActualKinStatus :  FB_CheckActualKinStatus;
   in_stKinToPlc   AT %I*    :  NciChannelToPlc;
   stAxesConfig              :  ST_KinAxes;
   eKinStatus                :  E_KINSTATUS;
END_VAR

fbFB_CheckActualKinStatus(
   bExecute            :=  TRUE,
   stAxesListReference :=  stAxesConfig,
   stKinRefIn          :=  in_stKinToPlc,
   eKinStatus          =>  eKinStatus                );

Kinematic Group State


FB_ConfigKinGroup

Interface

VAR_INPUT
   bExecute        :   BOOL;
   bCartesianMode  :   BOOL;
END_VAR

VAR_IN_OUT
   stAxesList  :   ST_KinAxes;
   stKinRefIn  :   NciChannelToPlc;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_ConfigKinGroup configures axes to the kinematic transformation, that is, configures the axes for the ACS (Axis Co-ordinate System, Joint) and MCS (Machine Co-ordinate System, Cartesian). The Function Block takes the ACS and MCS Axes defined in the stAxesList and configures them into the kinematic group of stKinRefIn.

All axes must first be enabled with MC_Power.

When bCartesianMode is FALSE, the ACS (Joint) axes can be moved directly.

When bCartesianMode is TRUE, movement in the MCS (Cartesian) axes will be transformed into the ACS (Joint) axes. The ACS axes cannot be moved directly.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

VAR
   io_X   :   AXIS_REF;
   io_Y   :   AXIS_REF;
   io_Z   :   AXIS_REF;
   io_M1  :   AXIS_REF;
   io_M2  :   AXIS_REF;
   io_M3  :   AXIS_REF;
   out_stPlcToKin AT %Q*:   NciChannelFromPlc;
   out_stPlcToItp AT %Q*:   NciChannelFromPlc;
   fbConfigKinGroup   :   FB_ConfigKinGroup;
   stAxesConfig   :   ST_KinAxes;
   bAllAxesReady  :   BOOL;
   bExecuteConfigKinGroup   :   BOOL;
   bUserConfigKinGroup   :   BOOL;
   bUserCartesianMode   :   BOOL;   := TRUE;
   (*true: cartesian mode - false: direct mode (without transformation) *)
END_VAR

(* read the axes id's from the cyclic interface so they can mapped later to the kinematic group *)
stAxesConfig.nAxisIdsAcs[1]     := io_M1.NcToPlc.AxisId;
stAxesConfig.nAxisIdsAcs[2]     := io_M2.NcToPlc.AxisId;
stAxesConfig.nAxisIdsAcs[3]     := io_M3.NcToPlc.AxisId;
stAxesConfig.nAxisIdsMcs[1]     := io_X.NcToPlc.AxisId;
stAxesConfig.nAxisIdsMcs[2]     := io_Y.NcToPlc.AxisId;
stAxesConfig.nAxisIdsMcs[3]     := io_Z.NcToPlc.AxisId;

IF bAllAxesReady AND bUserConfigKinGroup THEN
        bExecuteConfigKinGroup  := TRUE;
ELSE
        bExecuteConfigKinGroup  := FALSE;
END_IF

fbConfigKinGroup(       bExecute                :=      bExecuteConfigKinGroup ,
                bCartesianMode  :=      bUserCartesianMode ,
                stAxesList      :=      stAxesConfig,
                stKinRefIn      :=      in_stKinToPlc  );

Kinematic Group State

Note

The ACS axes must be enabled for the state to reach KinStatus_Ready. If the ACS Axes are not enabled, enable the axes and then call FB_ConfigKinGroup or FB_ResetKinGroup.


FB_ResetKinGroup

Interface

VAR_INPUT
   bExecute  :   BOOL;
   nItpChannelId  :   UDINT;
END_VAR

VAR_IN_OUT
   stKinRefIn  :   NciChannelToPlc;
   stAxesList  :   ST_KinAxes;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_ResetKinGroup will reset the Kinematic group. The ACS and MCS axes Ids are input in the structure stAxesList. All ACS and MCS axes will be reset. The input nItpChannelId can be used to specify the channel ID of the interpolator channel. The channel will be reset if the nItpChannelId is not 0.

If all Axes are enabled and the group was in Cartesian Mode the group will return to KinStatus_Ready.

If the group was not in Cartesian Mode the group will return to KinStatus_Empty.

If all the Axes are not enabled the group will remain in KinStatus_Empty.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

VAR
   fbFB_ResetKinGroup   :   FB_ResetKinGroup;
   stAxesConfig         :   stAxesConfig;
   in_stKinToPlc AT %I* :   NciChannelToPlc;
END_VAR

fbFB_ResetKinGroup(
   bExecute := TRUE,
   nItpChannelId := 3,
   stKinRefIn := in_stKinToPlc,
   stAxesList := stAxesConfig,
   bBusy=> ,
   bDone=> ,
   bError=> ,
   nErrorId=> );

Kinematic Group State


FB_CalcTrafo

Interface

VAR_INPUT
   bExecute  :   BOOL;
   bForward  :   BOOL;
   oidTrafo  :   UDINT;
END_VAR

VAR_IN_OUT
   stAxesPosIn   :   ARRAY[1..8] OF LREAL;
   stAxesPosOut  :   ARRAY[1..8] OF LREAL;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_CalcTrafo calculates the forward or reverse transformation. The Object Id of kinematic transformation object is supplied to the input oidTrafo. The state of the bForward input determines whether the forward or reverse transformation is calculated. The arrays stAxesPosIn and stAxesPosOut contain the input and output positions of the calculation.

In order to calculate a forward transformation, stAxesPosIn should be filled with joint positions. After execution stAxesPosOut will be populated with the corresponding cartesian positions.

In order to calculate a backward transformation, stAxesPosIn should be filled with Cartesian positions. After execution stAxesPosOut will be populated with the corresponding joint positions.

The transformation is not required to be configured with FB_ConfigKinGroup.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

The Scara Transformation has 4 ACS (joint) axes and 4 MCS (Cartesian) axes

Scara Transformation Example Configuration

Scara Transformation Example Object Id

VAR
   fbFB_CalcTrafo:  FB_CalcTrafo;
   stAxesPosIn   :  ARRAY[1..8]  OF  LREAL;
   stAxesPosOut  :  ARRAY[1..8]  OF  LREAL;
   bUserExecute  :  BOOL;
   bUserCalcFwdTrafo  :  BOOL;
END_VAR

fbFB_CalcTrafo(
   bExecute :=  bUserExecute,
   bForward :=  bUserCalcFwdTrafo,
   oidTrafo :=  16#01010072,
   stAxesPosIn :=  stAxesPosIn,
   stAxesPosOut :=  stAxesPosOut,
   bBusy=> ,
   bDone=> ,
   bError=> ,
   nErrorId=> );


FB_CalcMultiTrafo

Interface

VAR_INPUT
   bExecute  :   BOOL;
   bForward  :   BOOL;
   oidTrafo  :   UDINT;
   pDataIn   :   POINTER TO LREAL;
   nColumnsIn :   UDINT;
   pDataOut  :   POINTER TO LREAL;
   nColumnsOut :   UDINT;
   nLines    :   UDINT;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_CalcMultiTrafo calculates the forward or reverse transformation. The Object Id of the kinematic transformation object is supplied to the input oidTrafo. The state of the bForward input determines whether the forward or reverse transformation is calculated. The address of a table of input positions is input to pDataIn, correspondingly pDataOut is the address of a table of output positions.

In order to calculate a forward transformation, the table of input positions should be filled with joint positions. After execution the output table will be populated with the corresponding Cartesian positions.

In order to calculate a backward transformation, the table of input positions should be filled with Cartesian positions. After execution the output table will be populated with the corresponding joint positions.

The number of axes input to the transformation is input to nColumnsIn, the number of axes output from the transformation is input to nColumnsOut. The number of transformations to be calculated is input to nLines.

The transformation is not required to be configured with FB_ConfigKinGroup.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

The Scara Transformation has 4 ACS (joint) axes and 4 MCS (Cartesian) axes

Scara Transformation Example Configuration

Scara Transformation Example Object Id

VAR
   fbFB_CalcMultiTrafo : FB_CalcMultiTrafo;
   stAxesPosTabIn : ARRAY[1..3,1..4] OF LREAL
      := 0,0,100,20, (*Joint Values Pose 1*)
         15,15,50,10, (*Joint Values Pose 2*)
         30,30,25,5; (*Joint Values Pose 3*)
   stAxesPosTabOut :  ARRAY[1..3,1..4] OF LREAL;
   bUserCalcFwdTrafo : BOOL := TRUE;
   bUserExecute  : BOOL;

END_VAR

   fbFB_CalcMultiTrafo(
      bExecute := bUserExecute,
      bForward := bUserCalcFwdTrafo,
      oidTrafo := 16#01010072,
      pDataIn := ADR(stAxesPosTabIn),
      nColumnsIn := 4,
      pDataOut := ADR(stAxesPosTabOut),
      nColumnsOut := 4,
      nLines := 3,
      bBusy=> ,
      bDone=> ,
      bError=> ,
      nErrorId=> );


FB_LockTrafoParam

Interface

VAR_INPUT
   bExecute  :   BOOL;
   oidTrafo  :   UDINT;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_LockTrafoParam locks the transformation parameters so that they may not be written to. The Object Id of the kinematic transformation object is supplied to the input oidTrafo.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Example

Scara Transformation Example Object Id

VAR
   bUserExecute    : BOOL;
   fbFB_LockTrafoParam : FB_LockTrafoParam;
END_VAR

fbFB_LockTrafoParam(
   bExecute := bUserExecute,
   oidTrafo := 16#01010072,
   bBusy=>,
   bDone=>,
   bError=>,
   nErrorId=> );


FB_UnlockTrafoParam

Interface

VAR_INPUT
   bExecute  :   BOOL;
   oidTrafo  :   UDINT;
END_VAR

VAR_OUTPUT
   bBusy         :   BOOL;
   bDone         :   BOOL;
   bError        :   BOOL;
   nErrorId      :   UDINT;
END_VAR

Description

The Function Block FB_UnlockTrafoParam unlocks the transformation parameters so that they can be written to. The Object Id of kinematic transformation object is supplied to the input oidTrafo.

The command is executed at a positive edge at input bExecute.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

After the Unlock the Kinematic Parameters can be written from the PLC with ADSWRITE. The required Index Group is the Object Id and the Index Offset is the Parameter Id. Beware this is to be used with extreme caution. Redefining kinematic parameters can introduce discontinuities in the Kinematic chain. The written parameters are not persistent.

Example

Scara Transformation Example Object Id

VAR
   bUserExecute    : BOOL;
   fbFB_UnlockTrafoParam    : FB_UnlockTrafoParam;
END_VAR

fbFB_UnlockTrafoParam
   bExecute := bUserExecute,
   oidTrafo := 16#01010072,
   bBusy=>,
   bDone=>,
   bError=>,
   nErrorId=> );


E_KINSTATUS

TS5110 - TS5112 | TwinCAT Kinematic Transformation

TYPE E_KINSTATUS

[This is preliminary documentation and subject to change.]

TYPE E_KINSTATUS :
(
        KinStatus_Error,
        KinStatus_Empty,
        KinStatus_Unknown,
        KinStatus_StartPending,
        KinStatus_Ready
);
END_TYPE


ST_KinAxes

TS5110 - TS5112 | TwinCAT Kinematic Transformation

TYPE ST_KinAxes

[This is preliminary documentation and subject to change.]

TYPE ST_KinAxes :
STRUCT
        nAxisIdsMcs:    ARRAY[1..8] OF DWORD;
        nAxisIdsAcs:    ARRAY[1..8] OF DWORD;
END_STRUCT
END_TYPE

nAxisIdsMcs : A list of the Axes Ids of the Axes constituting the Machine Co-ordinate System

nAxisIdsAcs : A list of the Axes Ids of the Axes constituting the Axis Co-ordinate System


F_GetAcsMcsAxisIds

Interface

VAR_IN_OUT
   stAxesList  :   ST_KinAxes;
   stKinRefIn  :   NciChannelToPlc;
END_VAR

Input parameter

Input/output | Data type | Description
stAxesList | ST_KinAxes | A list of the Axis Ids for the Axis Coordinate System and the Machine Coordinate System
stKinRefIn | NciChannelToPlc | Structure of the cyclic channel interface from the Kinematic Channel to the PLC. This structure is only accessed for reading.

Return value

Data type | Description
UDINT | Error Number. This function is valid from version 6 of the cyclic interface. If the version is correct 0 is returned, otherwise an error.

Description

This function is valid from version 6 of the cyclic interface. If the version is correct 0 is returned, otherwise an error.

This function reads the configure ACS and MCS axes from the cyclic interface. The IDs are written into stAxesList.

Example

VAR
   stAxesList   : ST_KinAxes;
   stKinRefIn   AT %I*   : NciChannelToPlc;
   nErrId   :   UDINT;
END_VAR

nErrId := F_GetAcsMcsAxisIds(stAxesList, stKinRefIn);
IF nErrId=0 THEN
   ;(*Axes List is valid*)
END_IF


F_GetKinChnOperationState

Interface

VAR_IN_OUT
   stAxesList  :   ST_KinAxes;
END_VAR

Input parameter

Input/output | Data type | Description
stAxesList | ST_KinAxes | A list of the Axis Ids for the Axis Coordinate System and the Machine Coordinate System

Return value

Data type | Description
UDINT | Kinematic Channel Operation State. This function is valid from version 6 of the cyclic interface. If the version is incorrect an error is returned.

Description

This function is valid from version 6 of the cyclic interface. If the version is incorrect an error is returned.

This function returns the operation state of the Kinematic Channel.

Example

VAR
   stKinRefIn   AT %I*   :   NciChannelToPlc;
   nErrId   :   UDINT;
   nKinOperationState   :   E_KINSTATUS;END_VAR

IF F_GetKinChnOperationState(stKinRefIn)<>ErrTcNciItp_ItfVersion THEN
   nKinOperationState := UDINT_TO_UINT(F_GetKinChnOperationState(stKinRefIn));
ELSE
   nErrId:=F_GetKinChnOperationState(stKinRefIn);
END_IF


FB_TeachCoordinateSystem

Interface

VAR_INPUT
   bExecute   :   BOOL   := FALSE;
   oidTrafo   :   UDINT   := 0;
   vAxis1Point1   :   VECTOR;
   vAxis1Point2   :   VECTOR;
   vAxis2Point1   :   VECTOR;
   bParameteriseTrafo   :   BOOL   := FALSE;
   ePointSelection   :   E_KINPOINTSELECTION   := KinPointSel_XXY;
END_VAR
VAR_OUTPUT
   bBusy   :   BOOL	:= FALSE;
   bDone   :   BOOL	:= FALSE;
   bError   :   BOOL   := FALSE;
   nErrorId   :   UDINT   := 0;
   vTranslation   :   VECTOR;
   vRotation   :   VECTOR;
END_VAR

Description

This function block will calculate the parameters of a Static Transformation. The result can be written to a given StaticTrafo TCOM object.

The user should specify three points on a plane. Two points on one axis and one point on another axis. The default plane is XXY, two points on X axis and one point on Y axis. The plane can be selected with the ePointSelection input.

The points may be given directly. eg: input from a HMI, or from jogging the axes into position.

At a positive edge of bExecute the Transformation will be calculated. The result is output into vTranslation and vRotation. If bParameteriseTrafo is TRUE, the result will also be written into a StaticTrafo. The TCOM Object ID of the StaticTrafo must be specified at the input oidTrafo.

The bBusy output remains TRUE until the block has executed a command, although only for as long as the time specified at the tTimeOut input. While bBusy = TRUE, no new instruction will be accepted at the inputs. The output bError goes TRUE if an error occurs as the command is being executed. The command-specific error code is contained in nErrorId. The outputs are reset by carrying out a command at the inputs.

Note

The values written to the StaticTrafo are not persistent. A user program must save the values in PLC persistent variables, when the PLC starts the values should be written again to the StaticTrafo.

Example

VAR CONSTANT
   nOID_UCS_CONVEYOR   :   UDINT   :=   16#01010041;   (*Static Trafo Object ID*)
END_VAR

VAR
   vX1   :   VECTOR;
   vX2   :   VECTOR;
   vY1   :   VECTOR;
   bHmiCmdSaveSetPosAsX1   :   BOOL;
   bHmiCmdSaveSetPosAsX2   :   BOOL;
   bHmiCmdSaveSetPosAsY1   :   BOOL;
   bHmiCmdTeachUcsConveyor   :   BOOL;
   fbFB_TeachCoordinateSystem_Conveyor   :   FB_TeachCoordinateSystem;
END_VAR

(*Teach values direct from axis positions*)
IF bHmiCmdSaveSetPosAsX1 THEN
   vX1.m[1]   :=   Axis[4].NcToPlc.SetPos;
   vX1.m[2]   :=   Axis[5].NcToPlc.SetPos;
   vX1.m[3]   :=   Axis[6].NcToPlc.SetPos;
   bHmiCmdSaveSetPosAsX1   :=   FALSE;
END_IF

IF bHmiCmdSaveSetPosAsX2 THEN
   vX2.m[1]   :=   Axis[4].NcToPlc.SetPos;
   vX2.m[2]   :=   Axis[5].NcToPlc.SetPos;
   vX2.m[3]   :=   Axis[6].NcToPlc.SetPos;
   bHmiCmdSaveSetPosAsX2   :=   FALSE;
END_IF

IF bHmiCmdSaveSetPosAsY1 THEN
   vY1.m[1]   :=   Axis[4].NcToPlc.SetPos;
   vY1.m[2]   :=   Axis[5].NcToPlc.SetPos;
   vY1.m[3]   :=   Axis[6].NcToPlc.SetPos;
   bHmiCmdSaveSetPosAsY1   :=   FALSE;
END_IF

fbFB_TeachCoordinateSystem_Conveyor(
   bExecute:= bHmiCmdTeachUcsConveyor,
   oidTrafo:= nOID_UCS_CONVEYOR,
   vAxis1Point1 := vX1,
   vAxis1Point2 := vX2,
   vAxis2Point1 := vY1,
   bParameteriseTrafo:= TRUE,
   ePointSelection := KinPointSel_XXY,
   bBusy=> ,
   bDone=> ,
   bError=> ,
   nErrorId=> ,
   vTranslation=> ,
   vRotation=> );