Advanced Force Control

ContactAlign

Primitive Description and Usage

• Description: This primitive maintains force control and moves the robot in a predefined direction with a set force until it contacts with the environment while adjusting the robot’s position and orientation to make it compliant and align with the environment.

• Example Usage: Use this primitive when the robot’s final target pose is not a perfectly predefined pose. For example, when grasping an object whose position is uncertain, use this primitive so that the robot can first use the contact information to understand the spatial relationship between the target object and itself, and then perform grasping.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
contactAxis	Contact (force control) direction in TCP coordinate system, which should be along one of TCP’s principal axes [X, Y, Z]	VEC_3d	none	0 0 1 ∈ [-1 -1 -1 … 1 1 1]
contactVel	TCP linear velocity along the contact axis to the contact surface	DOUBLE	m/s	0.01 ∈ [0.001 … 0.03]
contactForce	Target force along the contact axis	DOUBLE	N	5 ∈ [5 … 30]
alignAxis	Alignment motion direction in TCP coordinate system. The robot performs alignment motion along the axis whose value is set to 1.	VEC_6i	none	0 0 0 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1]
alignVelScale	Velocity scale of alignment motion. Higher alignment velocity results in faster but less stable motion.	DOUBLE	none	0.2 ∈ [0.01 … 1.0]
deadbandScale	Deadband scale of the TCP force/moment. Robot will keep its stiffness until the TCP force/moment exceeds this deadband. Lower deadband results in more compliant but less stable motion.	DOUBLE	none	0.2 ∈ [0.01 … 1.0]

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
alignContacted	Flag to indicate that the alignment motion has been finished	BOOL	none
forwardDis	Forward distance from the start position to engaging the contact	DOUBLE	m

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
alignContacted	=	1

ForceHybrid

Primitive Description and Usage

• Description: This primitive controls the robot TCP to move linearly through waypoints to the target pose in hybrid motion/force control mode with force exerted in the force control direction.

• Example Usage: Use this primitive for better hybrid motion/force control performance when the robot needs to exert forces externally while running a trajectory.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
target*	Target TCP pose	COORD	m-deg	[traj_start* world*]
waypoint	Waypoint pose	ARRAY_COORD	m-deg	[traj_start* traj_goal* traj_prev* world*]
wrench	Force and torque applied on the waypoint	ARRAY_VEC_6d	N
vel	TCP linear velocity	DOUBLE	m/s	0.25 ∈ [0.001 … 2.2]
acc	TCP linear acceleration	DOUBLE	m/s^2	1.5 ∈ [0.1 … 3.0]
zoneRadius	Blending zone radius while TCP approximates the waypoints	TYPE	none	Z50 ∈ [ZFine Z1 Z5 Z10 Z15 Z20 Z30 Z40 Z50 Z60 Z80 Z100 Z150 Z200]
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 … 10]
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 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]
targetWrench*	Force and torque applied at target pose	VEC_6d	N	0.0 0.0 -5.0 0.0 0.0 0.0
angVel	TCP angular velocity	DOUBLE	deg/s	150.0 ∈ [10.0 … 500.0]
enableFixRefJntPos	Flag to indicate if fixed reference joint position is enabled. If this parameter is set to TRUE, the primitive will use the value of parameter [refJntPos] as the fixed joint reference position instead of the reference joint position of each waypoint during execution.	BOOL	none	0 ∈ [0 / 1]
refJntPos	Reference target joint positions. When the robot moves in Cartesian space, each joint will move as close as possible toward the reference position.	JPOS	deg	0.0 -40.0 0.0 90.0 0.0 40.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.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]
stiffScale	Robot stiffness scale during motion. The lower the stiffness scale, the more compliant the motion.	VEC_6d	none	1.0 1.0 1.0 1.0 1.0 1.0 ∈ [0.01 0.01 0.01 0.01 0.01 0.01 … 1.0 1.0 1.0 1.0 1.0 1.0]
enableMaxWrench	Flag to indicate if maximum contact wrench is enabled in each Cartesian direction: X, Y, Z, Rx, Ry, Rz	VEC_6i	none	0 0 0 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1]
maxContactWrench	Maximum contact wrench allowed for static collision along Fx, Fy, Fz, Mx, My, Mz	VEC_6d	N	150.0 150.0 150.0 40.0 40.0 40.0 ∈ [5.0 5.0 5.0 1.0 1.0 1.0 … 150.0 150.0 150.0 40.0 40.0 40.0]
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 coordinate 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 initial 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 describe 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 separate 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.

*Robot Joint Position Description

• The complete description of a joint waypoint (JPOS) is: A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the joint positions of the robot; X1 to X6 are the positions of external axes. You can only set the robot joint positions if external axes do not exist. For example: {jnt_def_1}.

• Add “:” to separate the waypoints. For example: {jnt_def_2}.

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

ForceComp

Primitive Description and Usage

• Description: This primitive controls the robot TCP to move compliantly through waypoints to the target pose. You can set appropriate values for parameters [stiffScale] and [maxContactWrench] to obtain desired compliant motion behaviors.

• Example Usage: Use this primitive when the robot has a chance to collide with an unknown environment during Cartesian movement or when you want to compliantly interact with the robot during movement.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
target*	Target TCP pose	COORD	m-deg	[traj_start* world*]
waypoints	Waypoints between initial and target poses	ARRAY_COORD	m-deg	[traj_start* traj_goal* traj_prev* world*]
vel	TCP linear velocity	DOUBLE	m/s	0.25 ∈ [0.001 … 2.2]
zoneRadius	Blending zone radius while TCP approximates the waypoints	TYPE	none	Z50 ∈ [ZFine Z1 Z5 Z10 Z15 Z20 Z30 Z40 Z50 Z60 Z80 Z100 Z150 Z200]
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 … 10]
compCoord	Reference coordinate system for the compliant motion	COORD	m-deg	0 0 0 0 0 0 WORLD WORLD_ORIGIN ∈ [world* tcp_start* tcp*]
stiffScale	Robot stiffness scale during motion. The lower the stiffness scale, the more compliant the motion.	VEC_6d	none	1.0 1.0 1.0 1.0 1.0 1.0 ∈ [0.01 0.01 0.01 0.01 0.01 0.01 … 1.0 1.0 1.0 1.0 1.0 1.0]
enableMaxWrench	Flag to indicate if maximum contact wrench is enabled in each Cartesian direction: X, Y, Z, Rx, Ry, Rz	VEC_6i	none	0 0 0 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 1 1 1]
maxContactWrench	Maximum contact wrench allowed for static collision along Fx, Fy, Fz, Mx, My, Mz	VEC_6d	N	150.0 150.0 150.0 40.0 40.0 40.0 ∈ [5.0 5.0 5.0 1.0 1.0 1.0 … 150.0 150.0 150.0 40.0 40.0 40.0]
acc	TCP linear acceleration	DOUBLE	m/s^2	1.5 ∈ [0.1 … 3.0]
angVel	TCP angular velocity	DOUBLE	deg/s	150.0 ∈ [10.0 … 500.0]
enableFixRefJntPos	Flag to indicate if fixed reference joint position is enabled. If this parameter is set to TRUE, the primitive will use the value of parameter [refJntPos] as the fixed joint reference position instead of the reference joint position of each waypoint during execution.	BOOL	none	0 ∈ [0 / 1]
refJntPos	Reference target joint positions. When the robot moves in Cartesian space, each joint will move as close as possible toward the reference position.	JPOS	deg	0.0 -40.0 0.0 90.0 0.0 40.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.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]

*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 coordinate 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 initial 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 describe 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 separate 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.

*Robot Joint Position Description

• The complete description of a joint waypoint (JPOS) is: A1 A2 A3 A4 A5 A6 A7 E1 E2 E3 E4 E5 E6. A1 to A7 are the joint positions of the robot; X1 to X6 are the positions of external axes. You can only set the robot joint positions if external axes do not exist. For example: {jnt_def_1}.

• Add “:” to separate the waypoints. For example: {jnt_def_2}.

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