Inconsistent behaviour of is_within_safety_limits and get_inverse_kin

Technical Questions
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