Inconsistent behaviour of is_within_safety_limits and get_inverse_kin

Technical Questions
1

Software version:5.6

Robot generation: E-series (not tested on CB-series)

Steps to reproduce:

We have identified a bug that makes it almost impossible to run our program properly on physical robots.

The documentation about is_within_safety_limits says “If a solution is found when applying the inverse kinematics to the given target TCP pose, this pose is considered reachable.”

However, it seems that is_within_safety_limits does NOT use the actual calibration data, while get_inverse_kin does . The attached example throws an exception popup when running on a physical robot, but it runs smoothly on a simulator. In the example I have attached the calibration data of a real robot of a customer, and also the contents of the same folder (.urcontrol) from my simulator. You can test the two different behaviours by simply replacing the .urcontrol folder on the simulator, and restart.

BugReportInverseKinematics.zip (292.2 KB)

set_tcp(p[0.1035,-0.045,0.185,0.0,0.0,1.1606439525762293])
epos= p[1.12704,0.197468,-0.493055,-2.22135,-2.22143,8.76663e-05]
if (is_within_safety_limits(epos)):
ikin = get_inverse_kin(epos)
textmsg("inverse kinematics: ", ikin)
# todo add project-specific calculations here
else:
textmsg(“out of reach”)
# skip calculations, point is out of reach
end

We run a lot of calculations in our program and use get_inverse_kinematics intensively. A calculation must not stop the robot with an exception popup at the customer in any circumstances, therefore we protect all these function calls with is_within_safety_limits. But the behaviour of these two functions are completely unpredictable, and the same program runs on one robot but not on another. We cannot guarantee that our program will run on a specific robot!

As a quick workaround, we have reduced the robot reach by stretching every pose by 2% before passing them to is_within_safety_limits:

if (is_within_safety_limits(stretch_by_2pct(target_pos)):
get_inverse_kin(target_pos)
…

This seems to work so far, but we need a solution that is 100% safe. A random exception popup at some customers is not acceptable.

Expected behavior:
expected a text message either “inverse kinematics: […]” or “out of reach” on the physical robot as well

Actual behavior:
the program throws an exception and opens a popup saying Script function get_inverse_kin unable to find a solution"

2

Hi Csaba,

Thank you for the detailed description!

The issue is confirmed and I will report it internally.

1 Like
3

Hi @csaba and all others struggling with this issue,

Due to dependencies into the safety part of our system, a fix will take a while. I have constructed this workaround URScript that uses the secondary interface to avoid the exception in the primary.

def is_within_reach_init(socket_name="sec_scoket"):
	socket_open("127.0.0.1",30002, socket_name=socket_name)
end

def double_to_str(x, mantissa_offset=1e-12):
	if x<0:
		sign=-1
		x=-x
	else:
		sign=1
	end
	exp=floor(log(2,x))
	mantissa = (pow(2.,-exp)*x-1)
	mantissa = mantissa+mantissa_offset
	x =sign*pow(2.,exp)*(1.0+mantissa)
	str=to_str(x-x%1)
	str=str_cat(str,".")
	local i=0
	x=norm(x%1)
	while i < log(10,pow(2,52-exp)):
		x=(x%1)*10
		
		str=str_cat(str,floor(x))
		i=i+1
	end
	return str
end

def list_to_str(l, mantissa_offset = 1e-12):
	str = "["
	local i = 0
	while i < get_list_length(l):
		str=str_cat(str, double_to_str(l[i], mantissa_offset = 1e-12))
		i=i+1
		str=str_cat(str, ",")
	end
	str = str_sub(str, 0, str_len(str)-1)	
	str=str_cat(str, "]")
	return str
end

def pose_to_str(p, mantissa_offset = 1e-12):
	str = "p"
	str = str_cat(str, list_to_str([p[0], p[1], p[2], p[3], p[4], p[5]], mantissa_offset = 1e-12))
	return str
end


def is_within_reach(x, qnear="undefined", maxPositionError="undefined", maxOrientationError="undefined", tcp="undefined", bool_reg_sync=126, bool_reg_fb=127, socket_name="sec_scoket", timeout=1):

	write_output_boolean_register(bool_reg_fb, False)
	write_output_boolean_register(bool_reg_sync, True)

	
	socket_send_line("sec is_with_reach_prog():", socket_name=socket_name)

	local cmd_str = "get_inverse_kin("
	cmd_str = str_cat(cmd_str, pose_to_str(x))

	if (to_str(qnear) != "undefined"):
		cmd_str = str_cat(cmd_str, ", qnear=")
		cmd_str = str_cat(cmd_str, list_to_str(qnear))
	end

	if (to_str(maxPositionError) != "undefined"):
		cmd_str = str_cat(cmd_str, ", maxPositionError=")
		cmd_str = str_cat(cmd_str, double_to_str(maxPositionError,0))
	end

	if (to_str(maxOrientationError) != "undefined"):
		cmd_str = str_cat(cmd_str, ", maxOrientationError=")
		cmd_str = str_cat(cmd_str, double_to_str(maxOrientationError,0))
	end


	if (to_str(tcp) != "undefined"):
		cmd_str = str_cat(cmd_str, ", tcp=")
		cmd_str = str_cat(cmd_str, pose_to_str(tcp,0))
	end
	cmd_str = str_cat(cmd_str, ")")
	socket_send_line(cmd_str, socket_name=socket_name)

	cmd_str = "write_output_boolean_register("
	cmd_str = str_cat(cmd_str, bool_reg_fb)
	cmd_str = str_cat(cmd_str, ", True)")
	socket_send_line(cmd_str, socket_name=socket_name)

	cmd_str = "write_output_boolean_register("
	cmd_str = str_cat(cmd_str, bool_reg_sync)
	cmd_str = str_cat(cmd_str, ", False)")
	socket_send_line(cmd_str, socket_name=socket_name)

	socket_send_line("end", socket_name=socket_name)

	local cnt=0
	while read_output_boolean_register(bool_reg_sync):
		if cnt > timeout/get_steptime():
			break
		end
		sync()
		cnt = cnt+1
	end
	return read_output_boolean_register(bool_reg_fb)
end
4

This bug is now resolved in version 5.9.4 and 3.14.3.

The function is_within_safety_limits(…) is now deprecated and replaced by the get_inverse_kin_has_solution(…).

I hope you will find it usefull @csaba and @nimal.kailasanathan :slight_smile:

2 Likes
5

Thanks Ebbe!

It works great!