Problem with Custom URScript for Press and Release Using Force Mode

Technical Questions URScript
1

def get_tcp_force_tool():
force_torque = get_tcp_force()
force_B = p[ force_torque[0], force_torque[1], force_torque[2], 0, 0, 0 ]
torque_B = p[ force_torque[3], force_torque[4], force_torque[5], 0, 0, 0 ]
tcp = get_actual_tcp_pose()
rotation_BT = p[ 0, 0, 0, tcp[3], tcp[4], tcp[5] ]
force_T = pose_trans( pose_inv(rotation_BT), force_B )
torque_T = pose_trans( pose_inv(rotation_BT), torque_B )
force_torque_T = p[ force_T[0], force_T[1], force_T[2], torque_T[0], torque_T[1], torque_T[2] ]
return force_torque_T
end

def press_and_release(fTarget):

delta = 0.0
stopl(1.0)
sleep(0.25)
zero_ftsensor()

force_mode(get_actual_tcp_pose(), [0,0,1,0,0,0], [0,0,fTarget,0,0,0], 2, [0.05, 0.05, 0.020, 0.717, 0.717, 0.717])

local fNow = p[0,0,0,0,0,0]
local n = 3
local fAvg = 0

while (fAvg + delta >-fTarget):
	local fAvg = 0
	local i = 0
	while i < n:
		fNow = get_tcp_force_tool()
		fAvg = fAvg + fNow[2]
		i=i+1
		sync()
	end
	fAvg = fAvg/n
                         write_output_float_register(0, fAvg)
end

end_force_mode()

stopl(3.0)
local pContact = get_actual_tcp_pose()
local pExit = pose_trans(pContact, p[0,0,-0.01, 0, 0, 0])
movel(pExit, v=0.25, a=2.5)

return fNow[2]

end

def press_and_release_v2(fTarget, tolerance=0.5, approach_dist=0.02):

stopl(0.5)
sleep(0.25)
zero_ftsensor()


tcp_pose = get_actual_tcp_pose()
pApproach = pose_trans(tcp_pose, p[0,0,approach_dist,0,0,0])
movel(pApproach, v=0.05, a=0.5)
popup("approach",title="point",blocking=True)


T = get_actual_tcp_pose()
z_tool= p[0,0,1,0,0,0]
z_axis=pose_trans(T,z_tool)
f_vec = [z_axis[0]*fTarget, z_axis[1]*fTarget, z_axis[2]*fTarget, 0, 0, 0]

force_mode(p[0,0,0,0,0,0], [0,0,1,0,0,0], f_vec, 2, [0.01,0.01,0.01,0.5,0.5,0.5])
local fNow=p[0,0,0,0,0,0]
fNow = get_tcp_force_tool()

while True:
	fNow = get_tcp_force_tool()
	write_output_float_register(0, fNow[2])

	if norm(fNow[2]) >= (fTarget - tolerance):
		end_force_mode()
		break
	end
	sync()
end


stopl(1.0)
pContact = get_actual_tcp_pose()
pExit = pose_trans(pContact, p[0,0,-0.03,0,0,0])
movel(pExit, v=0.25, a=2.5)
popup("Exited tool contact, release motion done", title="Debug End Motion", blocking=False)

return fNow[2]

end

I am trying to implement a custom URScript function to press against a surface with a specified force and then release. I wrote two versions of the function (press_and_release and press_and_release_v2) that use force_mode and read the TCP force using a helper function get_tcp_force_tool().

The goal is:

  • Move to an approach position.

  • Apply a target force along the TCP Z-axis.

  • Monitor the actual force until it reaches the target (within tolerance).

  • Stop force mode and retract.

Issue i am facing:

  1. How can I avoid singularities when using force_mode?
  2. Is there a recommended approach for pressing along the Z-axis without hitting singularities?