A collision handler is used to represent a known module and it allows the following functions:
Configure a collision entity based on a set of parameters describing the module
Automatically update the position and orientation of each object modelling the module based on the positions of the axes, for example.
The library provides different implementations of collision handlers but for all of them, the workflow is similar.
The workflow follows an example related to the configuration and update of FB_CollisionHandlerDelta3Ax:
|
Step |
Action |
|---|---|
|
1 |
Define an instance of the function block and a structure for storing the parameters. |
|
2 |
Prepare the structure stDelta3AxParameters with a valid set of parameters. For more information refer to ST_Delta3AxParameters - General Information. |
|
3 |
Set the parameters calling fbCollisionHandlerDelta3Ax.SetParameters(…) as follows: |
|
4 |
If the SetParameters call has been successful, the collision handler is now configured. In order to verify that, it is possible to check that fbCollisionHanlderDelta3Ax.xConfigured = TRUE. |
|
5 |
The collision handler is now configured, but in order to be usable with the collision and distance queries, perform at least one valid update. If fbCollisionHanlderDelta3Ax.xUpdated = TRUE, then the latest update of the collision handler has been successful. |
The basic collision handler interface IF_CollisionHandler does not include methods for the update of the collision handle. The reason for this is that each collision handler refers to a different module which means that it could require a different set of information in order to be updated.
Each function block implementing the generic interface IF_CollisionHandler also implements an additional interface containing the properties and methods required by a specific module. For example, FB_CollisionHandlerDelta3Ax implements the interface IF_CollisionHandlerDelta3Ax which contains properties and methods required for interacting with the collision handler of a Delta3Ax robot.
FB_CollisionHandlerDelta3Ax offers two methods to update its collision objects:
The method expects as input an array of LREAL containing the joint positions of the robot. This method uses the provided joint positions to update the position and orientation of each collision object inside the entity managed by the collision handler.
Given that fbCollisionHanlderDelta3Ax.xConfigured = TRUE, meaning that a successful call of SetParameters has been already performed, UpdateFromJointPositions can be used as follows:
//copy the current joint positions of the robot
FOR udiI := 1 TO COD.Gc_udiDelta3AxNumberOfJoints DO
alrRobotJointPositions[udiI] :=
ifFeedback.ifKinematic.ralrRefPositionJoint[ROB.ET_RobotComponent.AxisAll + udiI];
END_FOR
//update the collision handler for the robot based on the joint positions
fbCollisionHandlerDelta3Ax.UpdateFromJointPositions(
i_alrJointPositions := alrRobotJointPositions,
q_xError => xError,
q_etResult => etResult,
q_sResultMsg => sResultMsg
);
//check diagnostics here
IF xError THEN
//do something to handle the error
…
END_IF
In the example, ifFeedback is a valid instance of ROB.IF_RobotFeedback.
After a successful call of UpdateFromJointPositions, it must be fbCollisionHandlerDelta3Ax.xUpdated = TRUE. It is then possible to use the entity fbCollisionHandlerDelta3Ax.ifCollisionEntity with the collision and distance query functions.
The method expects a structure of type ST_Delta3AxKinematicsResult containing a valid result for the kinematic. This method uses the provided result of the kinematics to update the position and orientation of each collision object inside the entity managed by the collision handler.
In this case, the first step is to evaluate a valid result for the kinematics. This can be done using the methods that implement the direct and inverse kinematics that are called EvaluateDirectKinematics and EvaluateInverseKinematics.
Example for EvaluateDirectKinematics:
//copy the current joint positions of the robot
FOR udiI := 1 TO COD.Gc_udiDelta3AxNumberOfJoints DO
alrRobotJointPositions[udiI] :=
ifFeedback.ifKinematic.ralrRefPositionJoint[ROB.ET_RobotComponent.AxisAll + udiI];
END_FOR
//evaluate the direct kinematics providing the joint positions
fbCollisionHandlerDelta3Ax.EvaluateDirectKinematics(
i_alrJointPositions := alrRobotJointPositions,
q_xError => xError,
q_etResult => etResult,
q_sResultMsg => sResultMsg,
q_stResultLocal => stDelta3AxKinematicsResult
);
//check diagnostics here
IF xError THEN
//do something to handle the error
…
END_IF
In the example, ifFeedback is a valid instance of ROB.IF_RobotFeedback.
On a successful call of EvaluateDirectKinematics, stDelta3AxKinematicsResult contains a valid result for the kinematics and can be provided as input for UpdateFromKinematicsResult. For a valid result it must be stDelta3AxKinematicsResult.xIsResultValid = TRUE.
After a successful call of EvaluateDirectKinematics or EvaluateInverseKinematics, the result stDelta3AxKinematicsResult can be optionally used to update some additional collision objects defined by you that are linked to the motion of the robot but not directly managed by the collision handler. For detailed information refer to Adding a User Object or Group to a Collision Handler.
After this optional passage, it is possible to use stDelta3AxKinematicsResult to update the collision objects managed by the collision handler as follows:
//update the collision handler for the robot based on the result of the kinematics
fbCollisionHandlerDelta3Ax.UpdateFromKinematicsResult(
i_stKinematicsResult := stDelta3AxKinematicsResult,
q_xError => xError,
q_etResult => etResult,
q_sResultMsg => sResultMsg
);
After a successful call of UpdateFromKinematicsResult, it must be fbCollisionHandlerDelta3Ax.xUpdated = TRUE. It is then possible to use the entity fbCollisionHandlerDelta3Ax.ifCollisionEntity with the collision and distance query functions.