state.actual_TCP_pose results in wrong pose!

Technical Questions URScript
9

Hello @RobotNoob,

As promised I will answer your question while being back from holiday.
In PolyScope we scale the rotation vector so that it looks more stable and doesn’t flicker so much. The RTDE data is unscaled, but when scaled it corresponds to the GUI values shown in the customer’s screenshot. Anyway, both solutions represents same position. If you want to scale the RTDE reading this portion of code may be useful:

def tcp_pose_scal():
	v = get_actual_tcp_pose()
	pi=3.1416
	l = sqrt(pow(v[3],2)+pow(v[4],2)+pow(v[5],2))
	scale = 1-2*pi/l
	if  ( (norm(v[3]) >= 0.001 and v[3] < 0.0) or (norm(v[3]) < 0.001 and norm(v[4]) >= 0.001 and v[4] < 0.0) or (norm(v[3]) < 0.001 and norm(v[4]) < 0.001 and v[5] < 0.0) ):
		tcp_pose = p[v[0], v[1], v[2], scale*v[3], scale*v[4], scale*v[5]]
	else:
		tcp_pose = v
	end
		
	return tcp_pose
end

I’m hoping it answers your question. Wishing you all the best in developing your robotic solution.
@mbush, it is never too late to have explanation of something :slight_smile:

1 Like