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 is not a perfectly predefined pose. When grasping an object whose location is not fully known, the robot needs to use contact information to understand the spatial relationship between the object and itself.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
contactAxis	Approaching direction toward the contact surface 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 while moving to contact	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_3i	none	0 0 0 ∈ [0 0 0 … 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

ForceComp

Primitive Description and Usage

• Description: This primitive compliantly moves the robot to a target pose while passing through multiple waypoints. Users can specify the maximum velocity and acceleration of the TCP for the robot to move. Users need to set appropriate values for parameters stiffScale and maxContactForce to obtain desired compliant behaviors.

• Example Usage: Use this primitive when the robot has a chance to collide with an unknown environment during Cartesian movement or when users want to compliantly interact with the robot during Cartesian 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*]
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]
vel	TCP linear velocity	DOUBLE	m/s	0.25 ∈ [0.001 … 2.2]
acc	TCP linear acceleration	DOUBLE	m/s^2	10 ∈ [0.1 … 15.0]
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.0 … 1500.0]
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 0.0 0.0 0.0 ∈ [0.01 0.01 0.01 0.0 0.0 0.0 … 1.0 1.0 1.0 0.0 0.0 0.0]
enableMaxForce	Flag to indicate if maximum contact force is enabled in each Cartesian direction: X, Y, Z	VEC_6i	none	0 0 0 0 0 0 ∈ [0 0 0 0 0 0 … 1 1 1 0 0 0]
maxContactForce	Maximum contact force allowed for static collision along Fx, Fy, Fz	VEC_6d	N	150.0 150.0 150.0 0.0 0.0 0.0 ∈ [5.0 5.0 5.0 0.0 0.0 0.0 … 150.0 150.0 150.0 0.0 0.0 0.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

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	10 ∈ [0.1 … 15.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 0 0 0]
targetWrench*	Force and torque applied at target pose	VEC_6d	N	0.0 0.0 -5.0 0.0 0.0 0.0
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.0 … 1500.0]
stiffScale	Robot stiffness scale during motion. Its value ranges from 0.01 to 1.0, where 0.01 is the smallest stiffness.	VEC_6d	none	1.0 1.0 1.0 0.0 0.0 0.0 ∈ [0.01 0.01 0.01 0.0 0.0 0.0 … 1.0 1.0 1.0 0.0 0.0 0.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 0 0 0]
maxContactWrench	Maximum contact wrench allowed for static collision along Fx, Fy, Fz, Mx, My, Mz	VEC_6d	N	150.0 150.0 150.0 0.0 0.0 0.0 ∈ [5.0 5.0 5.0 0.0 0.0 0.0 … 150.0 150.0 150.0 0.0 0.0 0.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 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