Surface Finishing
Polish
Primitive Description and Usage
Description: This primitive provides high-accuracy hybrid motion/force control along user-specified trajectories. You can use the trajectory edited in the Trajectory Editor, and add entry and exit trajectories on top of the existing trajectory to smooth transition and reduce impact.
Example Usage: Use this primitive for flat or curved surface polishing and sanding. It can also be used for deburring.
Primitive Input Parameters
Input Parameter |
Description |
Type |
Unit |
Default Value & Range |
---|---|---|---|---|
trajFileName* |
Trajectory file name |
FILE |
none |
[.traj] |
enableEntryTraj |
Add entry trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
entryDis |
The length of the entry trajectory projected on the workpiece |
DOUBLE |
m |
0.050 ∈ [0.01 … 0.3] |
entryAngle |
Angle between entry path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
entryVel |
Linear velocity along the entry trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
enableExitTraj |
Add exit trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
exitDis |
The length of the exit trajectory projected on the workpiece |
DOUBLE |
m |
0.05 ∈ [0.01 … 0.3] |
exitAngle |
Angle between exit path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
exitVel |
Linear velocity along the exit trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
targetTolerLevel |
Tolerance level to determine if the robot has reached the target. 1 means to check with the smallest tolerance, 0 means no tolerance check. |
INT |
none |
0 ∈ [0 … 25] |
forceCoord |
Reference coordinate system for force control direction |
COORD |
m-deg |
0 0 0 0 0 0 TCP ONLINE ∈ [world* tcp_start* tcp*] |
forceAxis |
Activated axes of force control coordinate system to apply force or torque |
VEC_6i |
none |
0 0 1 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1] |
reverseTraj |
Flag to reverse the trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
trajRepeatTimes |
Number of times the trajectory is repeated |
INT |
none |
0 ∈ [0 … 20] |
enableILC |
Enable ILC (Interactive Learning Control) method to improve the force control performance |
BOOL |
none |
0 ∈ [0 / 1] |
ILCFileName |
Name of the ILC file |
STRING |
none |
ilcFile |
enableILCTraining |
Enable ILC training. If disabled, the ILC file will not be updated. |
BOOL |
none |
0 ∈ [0 / 1] |
restartILCTraining |
Reset the ILC training results |
BOOL |
none |
0 ∈ [0 / 1] |
ILCLearningRateType |
Type of learning rate strategy for the ILC algorithm. 0 means constant learning rate, 1 means linearly decaying learning rate, 2 means exponentially decaying learning rate. |
INT |
none |
0 ∈ [0 … 2] |
ILCLearningRate |
Initial learning rate of the ILC algorithm |
DOUBLE |
none |
0.5 ∈ [0 … 1.0] |
ILCDecayRate |
Decay rate of the ILC learning rate |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCForwardSteps |
Amount of control cycles that ILC would take to predict the disturbance |
INT |
none |
60 ∈ [0 … 200] |
angVel |
TCP angular velocity |
DOUBLE |
deg/s |
150.0 ∈ [10.0 … 500.0] |
configOptObj |
Weights of three configuration optimization objectives during robot motion, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the reference joint position. |
VEC_3d |
none |
0.0 0.0 0.5 ∈ [0.0 0.0 0.1 … 1.0 1.0 1.0] |
enableForceAutoRot |
Enable the automatic rotation of force control direction. The force control axis will always be perpendicular to the path forward direction at any moment, regardless of the forceAxis input specified before. |
BOOL |
none |
0 ∈ [0 / 1] |
forceRotAxis |
Rotation axis of the force control coordinate system |
VEC_3d |
none |
0 0 -1 ∈ [-1 -1 -1 … 1 1 1] |
enableContactAngle |
Enable the contact angle |
BOOL |
none |
0 ∈ [0 / 1] |
contactAngle |
Tilting angle of the polishing tool along the rotation axis of TCP coordinate system |
DOUBLE |
deg |
0.0 ∈ [-45.0 … 45.0] |
contactRotAxis |
Rotation axis of TCP coordinate system |
VEC_3d |
none |
0.0 0.0 0.0 ∈ [-1.0 -1.0 -1.0 … 1.0 1.0 1.0] |
contactRotRadius |
Rotation radius along the rotation axis. The contact point is defined by the rotation radius and rotation axis. |
DOUBLE |
m |
0.0 ∈ [0.0 … 0.3] |
enableTrajOverlay |
Enable the trajectory overlay function to overlay the wave trajectory on the taught trajectory. |
BOOL |
none |
0 ∈ [0 / 1] |
overlaidTrajType |
Overlaid trajectory type. 0 means sine wave; 1 means helix wave. |
INT |
none |
0 ∈ [0 … 1] |
amplitude |
Amplitude of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.02 ∈ [0 … 0.2] |
pitch |
Pitch of the helix wave trajectory |
DOUBLE |
m |
0.06 ∈ [0 … 0.2] |
lineSpace |
Wavelength of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.04 ∈ [0 … 0.2] |
maxVelForceDir |
Maximum movement velocity along force control direction |
VEC_3d |
m/s |
2.0 2.0 2.0 ∈ [0.005 0.005 0.005 … 2.0 2.0 2.0] |
enableTransLimit |
Enable TCP displacement limit in the translation (force control) direction after performing contact |
BOOL |
none |
0 ∈ [0 / 1] |
maxTransDisp |
Maximum TCP displacement in the translation (force control) direction after performing contact |
DOUBLE |
m |
0.5 ∈ [0.0 … 1.0] |
minTransDisp |
Minimum TCP displacement in the translation (force control) direction after performing contact |
DOUBLE |
m |
-0.5 ∈ [-1.0 … 0.0] |
enableOrientLimit |
Enable TCP rotation limit in the orientation direction after performing contact. Note: If enabled, only one translation axis and its orthogonal axes should be set for the parameter “forceAxis”(e.g. [0, 0, 1, 1, 1, 0]). |
BOOL |
none |
0 ∈ [0 / 1] |
toolRadius |
Polish tool radius |
DOUBLE |
m |
0.06 ∈ [0.03 … 0.2] |
maxOrientAngle |
Maximum rotation angle in the orientation direction after performing contact |
DOUBLE |
deg |
10.0 ∈ [0.1 … 20.0] |
*Parameters marked with an asterisk must be assigned a value prior to executing the primitive.
*Coordinate System Definition
Coordinate |
Definition |
Value Format |
---|---|---|
world |
WORLD coordinate system, which is a fixed Cartesian coordinate system located at the center of the robot base |
X Y Z Rx Ry Rz WORLD WORLD_ORIGIN |
work |
WORK coordinate system, which defines the position of the workpiece relative to the WORLD coordiante system |
X Y Z Rx Ry Rz WORK WorkCoordName |
tcp |
TCP coordinate system, which is located at the Tool Center Point relative to the center of robot flange |
X Y Z Rx Ry Rz TCP ONLINE |
tcp_start |
The fixed coordinate system which is located at the inital TCP pose of the primitive |
X Y Z Rx Ry Rz TCP START |
traj_start |
The offset of a waypoint relative to the initial TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ START |
traj_goal |
The offset of a waypoint relative to the target TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ GOAL |
traj_prev |
The offset of a waypoint relative to the previous waypoint in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ PREVIOUSWAYPOINT |
You can use the simplified value format above to describ a waypoint, while the complete description of a Cartesian waypoint is: X Y Z Rx Ry Rz ReferenceCoordinate A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the preferred joint positions of the robot; X1 to X6 are the target positions of external axes. The additional data can be added if necessary.
Add “:” to seperate the waypoints. For example: 0.2 0 0.3 0 180 0 WORLD WORLD_ORIGIN : 0.2 0.1 0.3 0 180 0 WORLD WORLD_ORIGIN.
Primitive State Parameters
State Parameter |
Description |
Type |
Unit |
---|---|---|---|
terminated |
The termination flag of the primitive. It is set to true if the primitive is terminated. |
BOOL |
none |
timePeriod |
The time spent on running the current primitive. |
DOUBLE |
s |
reachedTarget |
Flag to indicate if the robot has reached the target |
BOOL |
none |
waypointIndex |
Index of the current waypoint the robot just passed. 0 means the initial pose. |
INT |
none |
Primitive Output Parameters
Output Parameter |
Description |
Type |
Unit |
---|---|---|---|
tcpPoseOut |
The TCP pose when the primitive is terminated. It is represented in the world coordinate system. |
COORD |
m-deg |
Default Transition Condition
State Parameter |
Condition |
Value |
---|---|---|
reachedTarget |
= |
1 |
Grind
Primitive Description and Usage
Description: This primitive is similar to Polish but with higher motion stiffness. You can use the trajectory edited in the Trajectory Editor, add entry and exit trajectories on top of the existing trajectory to smooth transition and reduce impact, and set the TCP displacement in the force control direction to prevent the trajectory from deviating from the desired area.
Example Usage: Use this primitive for heavy-duty grinding tasks. The typical tools that work with this primitive are angle and straight electric grinders.
Primitive Input Parameters
Input Parameter |
Description |
Type |
Unit |
Default Value & Range |
---|---|---|---|---|
trajFileName* |
Trajectory file name |
FILE |
none |
[.traj] |
enableEntryTraj |
Add entry trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
entryDis |
The length of the entry trajectory projected on the workpiece |
DOUBLE |
m |
0.05 ∈ [0.01 … 0.3] |
entryAngle |
Angle between entry path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
entryVel |
Linear velocity along the entry trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.500.0] |
enableExitTraj |
Add exit trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
exitDis |
The length of the exit trajectory projected on the workpiece |
DOUBLE |
m |
0.05 ∈ [0.01 … 0.3] |
exitAngle |
Angle between exit path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
exitVel |
Linear velocity along the exit trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
targetTolerLevel |
Tolerance level to determine if the robot has reached the target. 1 means to check with the smallest tolerance, 0 means no tolerance check. |
INT |
none |
0 ∈ [0 … 25] |
forceCoord |
Reference coordinate system for force control direction |
COORD |
m-deg |
0 0 0 0 0 0 TCP ONLINE ∈ [world* tcp_start* tcp*] |
forceAxis |
Activated axes of force control coordinate system to apply force or torque |
VEC_6i |
none |
0 0 1 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1] |
reverseTraj |
Flag to reverse the trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
trajRepeatTimes |
Number of times the trajectory is repeated |
INT |
none |
0 ∈ [0 … 20] |
enableILC |
Enable ILC(Interactive Learning Control) method to improve the force control performance |
BOOL |
none |
0 ∈ [0 / 1] |
ILCFileName |
Name of the ILC file |
STRING |
none |
ilcFile |
enableILCTraining |
Enable ILC training. If disabled, the ILC file will not be updated. |
BOOL |
none |
0 ∈ [0 / 1] |
restartILCTraining |
Reset the ILC training results |
BOOL |
none |
0 ∈ [0 / 1] |
ILCLearningRateType |
Type of learning rate strategy for the ILC algorithm. 0 means constant learning rate, 1 means linearly decaying learning rate, 2 means exponentially decaying learning rate. |
INT |
none |
1 ∈ [0 … 2] |
ILCPosLearningRate |
Initial learning rate of the ILC algorithm for position feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCPosDecayRate |
Decay rate of the ILC learning rate for position feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCVelLearningRate |
Initial learning rate of the ILC algorithm for velocity feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCVelDecayRate |
Decay rate of the ILC learning rate for velocity feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCAccLearningRate |
Initial learning rate of the ILC algorithm for acceleration feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCAccDecayRate |
Decay rate of the ILC learning rate for acceleration feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCForwardSteps |
Amount of control cycles that ILC would take to predict the disturbance |
INT |
none |
0 ∈ [0 … 200] |
angVel |
TCP angular velocity |
DOUBLE |
deg/s |
150.0 ∈ [10.0 … 500.0] |
configOptObj |
Weights of three configuration optimization objectives during robot motion, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the reference joint position. |
VEC_3d |
none |
0.0 0.0 0.5 ∈ [0.0 0.0 0.1 … 1.0 1.0 1.0] |
enableForceAutoRot |
Enable the automatic rotation of force control direction. The force control axis will always be perpendicular to the path forward direction at any moment, regardless of the forceAxis input specified before. |
BOOL |
none |
0 ∈ [0 / 1] |
forceRotAxis |
Rotation axis of the force control coordinate system |
VEC_3d |
none |
0 0 -1 ∈ [-1 -1 -1 … 1 1 1] |
enableContactAngle |
Enable the contact angle |
BOOL |
none |
0 ∈ [0 / 1] |
contactAngle |
Tilting angle of the polishing tool along the rotation axis of TCP coordinate system |
DOUBLE |
deg |
0.0 ∈ [-45.0 … 45.0] |
contactRotAxis |
Rotation axis of TCP coordinate system |
VEC_3d |
none |
0.0 0.0 0.0 ∈ [-1.0 -1.0 -1.0 … 1.0 1.0 1.0] |
contactRotRadius |
Rotation radius along the rotation axis. The contact point is defined by the rotation radius and rotation axis. |
DOUBLE |
m |
0.0 ∈ [0.0 … 0.3] |
enableTrajOverlay |
Enable the trajectory overlay function to overlay the wave trajectory on the taught trajectory. |
BOOL |
none |
0 ∈ [0 / 1] |
overlaidTrajType |
Overlaid trajectory type. 0 means sine wave; 1 means helix wave. |
INT |
none |
0 ∈ [0 … 1] |
amplitude |
Amplitude of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.02 ∈ [0 … 0.2] |
pitch |
Pitch of the helix wave trajectory |
DOUBLE |
m |
0.06 ∈ [0 … 0.2] |
lineSpace |
Wavelength of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.04 ∈ [0 … 0.2] |
maxVelForceDir |
Maximum movement velocity along force control direction |
VEC_3d |
m/s |
2.0 2.0 2.0 ∈ [0.005 0.005 0.005 … 2.0 2.0 2.0] |
enableTransLimit |
Enable TCP displacement limit in the translation (force control) direction after performing contact |
BOOL |
none |
0 ∈ [0 / 1] |
maxTransDisp |
Maximum TCP displacement in the translation (force control) direction after performing contact |
DOUBLE |
m |
0.5 ∈ [0.0 … 1.0] |
minTransDisp |
Minimum TCP displacement in the translation (force control) direction after performing contact |
DOUBLE |
m |
-0.5 ∈ [-1.0 … 0.0] |
enableOrientLimit |
Enable TCP rotation limit in the orientation direction after performing contact. Note: If enabled, only one translation axis and its orthogonal axes should be set for the parameter “forceAxis”(e.g. [0, 0, 1, 1, 1, 0]). |
BOOL |
none |
0 ∈ [0 / 1] |
toolRadius |
Polish tool radius |
DOUBLE |
m |
0.06 ∈ [0.03 … 0.2] |
maxOrientAngle |
Maximum rotation angle in the orientation direction after performing contact |
DOUBLE |
deg |
10.0 ∈ [0.1 … 20.0] |
*Parameters marked with an asterisk must be assigned a value prior to executing the primitive.
*Coordinate System Definition
Coordinate |
Definition |
Value Format |
---|---|---|
world |
WORLD coordinate system, which is a fixed Cartesian coordinate system located at the center of the robot base |
X Y Z Rx Ry Rz WORLD WORLD_ORIGIN |
work |
WORK coordinate system, which defines the position of the workpiece relative to the WORLD coordiante system |
X Y Z Rx Ry Rz WORK WorkCoordName |
tcp |
TCP coordinate system, which is located at the Tool Center Point relative to the center of robot flange |
X Y Z Rx Ry Rz TCP ONLINE |
tcp_start |
The fixed coordinate system which is located at the inital TCP pose of the primitive |
X Y Z Rx Ry Rz TCP START |
traj_start |
The offset of a waypoint relative to the initial TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ START |
traj_goal |
The offset of a waypoint relative to the target TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ GOAL |
traj_prev |
The offset of a waypoint relative to the previous waypoint in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ PREVIOUSWAYPOINT |
You can use the simplified value format above to describ a waypoint, while the complete description of a Cartesian waypoint is: X Y Z Rx Ry Rz ReferenceCoordinate A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the preferred joint positions of the robot; X1 to X6 are the target positions of external axes. The additional data can be added if necessary.
Add “:” to seperate the waypoints. For example: 0.2 0 0.3 0 180 0 WORLD WORLD_ORIGIN : 0.2 0.1 0.3 0 180 0 WORLD WORLD_ORIGIN.
Primitive State Parameters
State Parameter |
Description |
Type |
Unit |
---|---|---|---|
terminated |
The termination flag of the primitive. It is set to true if the primitive is terminated. |
BOOL |
none |
timePeriod |
The time spent on running the current primitive. |
DOUBLE |
s |
reachedTarget |
Flag to indicate if the robot has reached the target |
BOOL |
none |
waypointIndex |
Index of the current waypoint the robot just passed. 0 means the initial pose. |
INT |
none |
Primitive Output Parameters
Output Parameter |
Description |
Type |
Unit |
---|---|---|---|
tcpPoseOut |
The TCP pose when the primitive is terminated. It is represented in the world coordinate system. |
COORD |
m-deg |
Default Transition Condition
State Parameter |
Condition |
Value |
---|---|---|
reachedTarget |
= |
1 |
PolishECP
Primitive Description and Usage
Description: This primitive allows the robot to polish workpieces attached to the robot on an external polishing/sanding/grinding instrument. ECP stands for External Control Point or External TCP. Similar to Polish, this primitive also provides hybrid motion/force control along user-specified trajectories, but the trajectories and force control axes are defined in the ECP coordinate system.
Example Usage: Use this primitive for tasks that involve large polishing/sanding/grinding tools. In these tasks, the workpiece can be mounted on the robot’s flange and be put in contact with the tools for polishing.
Primitive Input Parameters
Input Parameter |
Description |
Type |
Unit |
Default Value & Range |
---|---|---|---|---|
trajFileName* |
Trajectory file name |
FILE |
none |
[.traj] |
ECPCoord* |
ECP coordinate system to be used. An offset to the ECP can also be specified. |
COORD |
m-deg |
[world* work*] |
enableEntryTraj |
Add entry trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
entryDis |
Length of the entry trajectory projected on the workpiece |
DOUBLE |
m |
0.050 ∈ [0.01 … 0.3] |
entryAngle |
Angle between entry path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
entryVel |
Linear velocity along the entry trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
enableExitTraj |
Add exit trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
exitDis |
Length of the exit trajectory projected on the workpiece |
DOUBLE |
m |
0.05 ∈ [0.01 … 0.3] |
exitAngle |
Angle between exit path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
exitVel |
Linear velocity along the exit trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
targetTolerLevel |
Tolerance level to determine if the robot has reached the target. 1 means to check with the smallest tolerance, 0 means no tolerance check. |
INT |
none |
0 ∈ [0 … 25] |
forceAxis |
Activated axes of ECP coordinate to apply force or torque |
VEC_6i |
none |
0 0 1 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1] |
trajRepeatTimes |
Number of times the trajectory is repeated |
INT |
none |
0 ∈ [0 … 20] |
reverseTraj |
Flag to reverse the trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
enableILC |
Enable ILC(Interactive Learning Control) method to improve the force control performance |
BOOL |
none |
0 ∈ [0 / 1] |
ILCFileName |
Name of the ILC file |
STRING |
none |
ilcFile |
enableILCTraining |
Enable ILC training. If disabled, the ILC file will not be updated. |
BOOL |
none |
0 ∈ [0 / 1] |
restartILCTraining |
Reset the ILC training results |
BOOL |
none |
0 ∈ [0 / 1] |
ILCLearningRateType |
Type of learning rate strategy for the ILC algorithm. 0 means constant learning rate, 1 means linearly decaying learning rate, 2 means exponentially decaying learning rate. |
INT |
none |
0 ∈ [0 … 2] |
ILCLearningRate |
Initial learning rate of the ILC algorithm |
DOUBLE |
none |
0.5 ∈ [0 … 1.0] |
ILCDecayRate |
Decay rate of the ILC learning rate |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCforwardSteps |
Amount of control cycles that ILC would take to predict the disturbance |
INT |
none |
60 ∈ [0 … 200] |
angVel |
TCP angular velocity |
DOUBLE |
deg/s |
150.0 ∈ [10.0 … 500.0] |
configOptObj |
Weights of three configuration optimization objectives during robot motion, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the reference joint position. |
VEC_3d |
none |
0.0 0.0 0.5 ∈ [0.0 0.0 0.1 … 1.0 1.0 1.0] |
enableTrajOverlay |
Enable the trajectory overlay function to overlay the wave trajectory on the taught trajectory. |
BOOL |
none |
0 ∈ [0 / 1] |
overlaidTrajType |
Overlaid trajectory type. 0 means sine wave; 1 means helix wave. |
INT |
none |
0 ∈ [0 … 1] |
amplitude |
Amplitude of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
pitch |
Pitch of the helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
lineSpace |
Wavelength of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
maxVelForceDir |
Maximum movement velocity along force control direction |
VEC_3d |
m/s |
2.0 2.0 2.0 ∈ [0.005 0.005 0.005 … 2.0 2.0 2.0] |
*Parameters marked with an asterisk must be assigned a value prior to executing the primitive.
*Coordinate System Definition
Coordinate |
Definition |
Value Format |
---|---|---|
world |
WORLD coordinate system, which is a fixed Cartesian coordinate system located at the center of the robot base |
X Y Z Rx Ry Rz WORLD WORLD_ORIGIN |
work |
WORK coordinate system, which defines the position of the workpiece relative to the WORLD coordiante system |
X Y Z Rx Ry Rz WORK WorkCoordName |
tcp |
TCP coordinate system, which is located at the Tool Center Point relative to the center of robot flange |
X Y Z Rx Ry Rz TCP ONLINE |
tcp_start |
The fixed coordinate system which is located at the inital TCP pose of the primitive |
X Y Z Rx Ry Rz TCP START |
traj_start |
The offset of a waypoint relative to the initial TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ START |
traj_goal |
The offset of a waypoint relative to the target TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ GOAL |
traj_prev |
The offset of a waypoint relative to the previous waypoint in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ PREVIOUSWAYPOINT |
You can use the simplified value format above to describ a waypoint, while the complete description of a Cartesian waypoint is: X Y Z Rx Ry Rz ReferenceCoordinate A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the preferred joint positions of the robot; X1 to X6 are the target positions of external axes. The additional data can be added if necessary.
Add “:” to seperate the waypoints. For example: 0.2 0 0.3 0 180 0 WORLD WORLD_ORIGIN : 0.2 0.1 0.3 0 180 0 WORLD WORLD_ORIGIN.
Primitive State Parameters
State Parameter |
Description |
Type |
Unit |
---|---|---|---|
terminated |
The termination flag of the primitive. It is set to true if the primitive is terminated. |
BOOL |
none |
timePeriod |
The time spent on running the current primitive. |
DOUBLE |
s |
reachedTarget |
Flag to indicate if the robot has reached the target |
BOOL |
none |
waypointIndex |
Index of the current waypoint the robot just passed. 0 means the initial pose. |
INT |
none |
Primitive Output Parameters
Output Parameter |
Description |
Type |
Unit |
---|---|---|---|
tcpPoseOut |
The TCP pose when the primitive is terminated. It is represented in the world coordinate system. |
COORD |
m-deg |
Default Transition Condition
State Parameter |
Condition |
Value |
---|---|---|
reachedTarget |
= |
1 |
GrindECP
Primitive Description and Usage
Description: This primitive allows the robot to polish workpieces attached to the robot on an external polishing/sanding/grinding instrument with high motion stiffness. ECP stands for External Control Point or External TCP. Similar to Grind, this primitive also provides hybrid motion/force control along user-specified trajectories, but the trajectories and force control axes are defined in the ECP coordinate system.
Example Usage: Use this primitive for tasks that involve large polishing/sanding/grinding tools. In these tasks, the workpiece can be mounted on the robot’s flange and be put in contact with the tools for grinding.
Primitive Input Parameters
Input Parameter |
Description |
Type |
Unit |
Default Value & Range |
---|---|---|---|---|
trajFileName* |
Trajectory file name |
FILE |
none |
[.traj] |
ECPCoord* |
ECP coordinate system to be used. An offset to the ECP can also be specified. |
COORD |
m-deg |
[world* work*] |
enableEntryTraj |
Add entry trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
entryDis |
Length of the entry trajectory projected on the workpiece |
DOUBLE |
m |
0.050 ∈ [0.01 … 0.3] |
entryAngle |
Angle between entry path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
entryVel |
Linear velocity along the entry trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
enableExitTraj |
Add exit trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
exitDis |
Length of the exit trajectory projected on the workpiece |
DOUBLE |
m |
0.05 ∈ [0.01 … 0.3] |
exitAngle |
Angle between exit path and the workpiece |
DOUBLE |
deg |
5.0 ∈ [1.0 … 90.0] |
exitVel |
Linear velocity along the exit trajectory |
DOUBLE |
m/s |
0.05 ∈ [0.001 … 0.5] |
targetTolerLevel |
Tolerance level to determine if the robot has reached the target. 1 means to check with the smallest tolerance, 0 means no tolerance check. |
INT |
none |
0 ∈ [0 … 25] |
forceAxis |
Activated axes of ECP coordinate to apply force or torque |
VEC_6i |
none |
0 0 1 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1] |
trajRepeatTimes |
Number of times the trajectory is repeated |
INT |
none |
0 ∈ [0 … 20] |
reverseTraj |
Flag to reverse the trajectory |
BOOL |
none |
0 ∈ [0 / 1] |
enableILC |
Enable ILC(Interactive Learning Control) method to improve the force control performance |
BOOL |
none |
0 ∈ [0 / 1] |
ILCFileName |
Name of the ILC file |
STRING |
none |
ilcFile |
enableILCTraining |
Enable ILC training. If disabled, the ILC file will not be updated. |
BOOL |
none |
0 ∈ [0 / 1] |
restartILCTraining |
Reset the ILC training results |
BOOL |
none |
0 ∈ [0 / 1] |
ILCLearningRateType |
Type of learning rate strategy for the ILC algorithm. 0 means constant learning rate, 1 means linearly decaying learning rate, 2 means exponentially decaying learning rate. |
INT |
none |
1 ∈ [0 … 2] |
ILCPosLearningRate |
Initial learning rate of the ILC algorithm for position feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCPosDecayRate |
Decay rate of the ILC learning rate for position feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCVelLearningRate |
Initial learning rate of the ILC algorithm for velocity feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCVelDecayRate |
Decay rate of the ILC learning rate for velocity feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCAccLearningRate |
Initial learning rate of the ILC algorithm for acceleration feedforward |
DOUBLE |
none |
0.3 ∈ [0 … 1.0] |
ILCAccDecayRate |
Decay rate of the ILC learning rate for acceleration feedforward |
DOUBLE |
none |
0.015 ∈ [0 … 1.0] |
ILCforwardSteps |
Amount of control cycles that ILC would take to predict the disturbance |
INT |
none |
0 ∈ [0 … 200] |
angVel |
TCP angular velocity |
DOUBLE |
deg/s |
150.0 ∈ [10.0 … 500.0] |
configOptObj |
Weights of three configuration optimization objectives during robot motion, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the reference joint position. |
VEC_3d |
none |
0.0 0.0 0.5 ∈ [0.0 0.0 0.1 … 1.0 1.0 1.0] |
enableTrajOverlay |
Enable the trajectory overlay function to overlay the wave trajectory on the taught trajectory. |
BOOL |
none |
0 ∈ [0 / 1] |
overlaidTrajType |
Overlaid trajectory type. 0 means sine wave; 1 means helix wave. |
INT |
none |
0 ∈ [0 … 1] |
amplitude |
Amplitude of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
pitch |
Pitch of the helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
lineSpace |
Wavelength of the overlaid sine wave or helix wave trajectory |
DOUBLE |
m |
0.01 ∈ [0.006 … 0.2] |
maxVelForceDir |
Maximum movement velocity along force control direction |
VEC_3d |
m/s |
2.0 2.0 2.0 ∈ [0.005 0.005 0.005 … 2.0 2.0 2.0] |
*Parameters marked with an asterisk must be assigned a value prior to executing the primitive.
*Coordinate System Definition
Coordinate |
Definition |
Value Format |
---|---|---|
world |
WORLD coordinate system, which is a fixed Cartesian coordinate system located at the center of the robot base |
X Y Z Rx Ry Rz WORLD WORLD_ORIGIN |
work |
WORK coordinate system, which defines the position of the workpiece relative to the WORLD coordiante system |
X Y Z Rx Ry Rz WORK WorkCoordName |
tcp |
TCP coordinate system, which is located at the Tool Center Point relative to the center of robot flange |
X Y Z Rx Ry Rz TCP ONLINE |
tcp_start |
The fixed coordinate system which is located at the inital TCP pose of the primitive |
X Y Z Rx Ry Rz TCP START |
traj_start |
The offset of a waypoint relative to the initial TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ START |
traj_goal |
The offset of a waypoint relative to the target TCP pose in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ GOAL |
traj_prev |
The offset of a waypoint relative to the previous waypoint in the TCP coordinate system |
X Y Z Rx Ry Rz TRAJ PREVIOUSWAYPOINT |
You can use the simplified value format above to describ a waypoint, while the complete description of a Cartesian waypoint is: X Y Z Rx Ry Rz ReferenceCoordinate A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the preferred joint positions of the robot; X1 to X6 are the target positions of external axes. The additional data can be added if necessary.
Add “:” to seperate the waypoints. For example: 0.2 0 0.3 0 180 0 WORLD WORLD_ORIGIN : 0.2 0.1 0.3 0 180 0 WORLD WORLD_ORIGIN.
Primitive State Parameters
State Parameter |
Description |
Type |
Unit |
---|---|---|---|
terminated |
The termination flag of the primitive. It is set to true if the primitive is terminated. |
BOOL |
none |
timePeriod |
The time spent on running the current primitive. |
DOUBLE |
s |
reachedTarget |
Flag to indicate if the robot has reached the target |
BOOL |
none |
waypointIndex |
Index of the current waypoint the robot just passed. 0 means the initial pose. |
INT |
none |
Primitive Output Parameters
Output Parameter |
Description |
Type |
Unit |
---|---|---|---|
tcpPoseOut |
The TCP pose when the primitive is terminated. It is represented in the world coordinate system. |
COORD |
m-deg |
Default Transition Condition
State Parameter |
Condition |
Value |
---|---|---|
reachedTarget |
= |
1 |