Basic Force Control

Basic Force Control

ZeroFTSensor

Primitive Description and Usage

• Description: This primitive is used to reset the Cartesian force/torque offset to zero. It is usually used before force control primitives. It can be used with or without a 6DoF force/torque sensor. The robot should be static enough (i.e. the previous primitive cannot trigger extremely aggressive motions) when entering this primitive. During the calibration, the robot should neither be touched nor contact with the environment.

• Example Usage: Use this primitive to reset the Cartesian force/torque offset (to zero) before polishing or contacts.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
dataCollectTime	The amount of time used to collect force/torque offset prior to resetting the FT sensor	DOUBLE	s	0.2 ∈ [0.1 … 1.0]
enableStaticCheck	Enable checking if the robot is static or not	BOOL	none	0 ∈ [0 / 1]

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

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

Contact

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. The robot stops immediately when its contact force with the object reaches the set max contact force value. Users can specify the moving speed and contact force as input parameters. Users can specify multiple intermediate waypoints and the moving speed while the robot is reaching the surface. ZeroFTSensor should always be used before executing Contact.

• Example Usage: Use this primitive prior to tasks when the robot contacts with the environment. For example, in polishing or assembly tasks, the robot’s tool needs to make contact with the parts before performing any polishing or peg-in-hole tasks.

Primitive Input Parameters

Input Parameter	Description	Type	Unit	Default Value & Range
waypoint	Waypoint pose	ARRAY_COORD	m-deg	[traj_start* traj_goal* traj_prev* world*]
vel	TCP linear velocity	ARRAY_DOUBLE	m/s	[0.0005 … 2.2]
acc	TCP linear acceleration	ARRAY_DOUBLE	m/s^2	[0.1 … 3.0]
zoneRadius	Blending zone radius while TCP approximating the waypoints	ARRAY_TYPE	mm	[ZFine Z1 Z5 Z10 Z15 Z20 Z30 Z40 Z50 Z60 Z80 Z100 Z150 Z200]
enableSuperArc	Enable axis rotating between adjacent orientations	ARRAY_BOOL	none
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.0 … 1500.0]
contactVel	TCP linear velocity while moving to contact	DOUBLE	m/s	0.02 ∈ [0.001 … 0.1]
contactCoord	Reference coordinate system for the contact direction	TYPE	none	world ∈ [world tcp]
contactDir	Approaching direction toward the contact surface in the contact coordinate system	VEC_3d	none	0.0 0.0 -1.0
maxContactForce	Maximum contact force. The primitive will be terminated when external force exceeds this limit.	DOUBLE	N	5.0 ∈ [1.0 … 120]
enableFineContact	Enabling this mode allows the robot automatically adjust the velocity while engaging the contact. Otherwise, the robot just applies the constant velocity.	BOOL	none	1 ∈ [0 / 1]

*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 ONELINE
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

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
curContactForce	Current force in the contact direction	DOUBLE	N
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
terminated	=	1

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. For example, (0,1,0,1,0,0) means that the robot executes alignment motion at Y and Rx axis of TCP coordinate.	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 would keep its stiffness until the TCP force/moment exceed 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

MoveHybrid

Primitive Description and Usage

• Description: This primitive uses hybrid motion/force control to move the robot linearly between waypoints in motion control directions and to exert force in force control directions.

• Example Usage: Use this primitive to do polishing on a flat surface along trajectories with straight-line segments or to do polishing along edges with straight-line shapes. If the surface is curved or users want more advanced functionalities for better motion/force control, it is recommended to use the primitives under the Polish category.

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 TCP coordinate	ARRAY_VEC_6d	N
vel	TCP linear velocity	DOUBLE	m/s	0.5 ∈ [0.0005 … 2.2]
acc	TCP linear acceleration	DOUBLE	m/s^2	1.5 ∈ [0.1 … 3.0]
zoneRadius	Blending zone radius while TCP approximating the waypoints	TYPE	mm	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
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.0 … 1500.0]
preferJntPos	Preferred target joint positions. When the robot moves in Cartesian space, each joint will move as close as possible toward the preferred position.	VEC_7d	deg	0.0 -40.0 0.0 90.0 0.0 40.0 0.0 ∈ [-160 -130 -170 -107 -170 -80 -170 … 160 130 170 154 170 260 170]
configOptObj	Weights of three configuration optimization objectives, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the preferred joint position.	VEC_3d	none	0.0 0.0 0.5 ∈ [0.0 0.0 0.1 … 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 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 ONELINE
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

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

MoveComp

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 maxContactWrench 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*]
vel	TCP linear velocity	DOUBLE	m/s	0.5 ∈ [0.0005 … 2.2]
zoneRadius	Blending zone radius while TCP approximating the waypoints	TYPE	mm	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 … 2.0 2.0 2.0 2.0 2.0 2.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]
jerk	TCP linear jerk	DOUBLE	m/s^3	200.0 ∈ [50.0 … 1500.0]
preferJntPos	Preferred target joint positions. When the robot moves in Cartesian space, each joint will move as close as possible toward the preferred position.	VEC_7d	deg	0.0 -40.0 0.0 90.0 0.0 40.0 0.0 ∈ [-160 -130 -170 -107 -170 -80 -170 … 160 130 170 154 170 260 170]
configOptObj	Weights of three configuration optimization objectives, which respectively are to make the robot easier to translate in Cartesian space, easier to rotate in Cartesian space, and closer to the preferred 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 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 ONELINE
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

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