state.actual_TCP_pose results in wrong pose!

Hi all,
I am using rtde package to communicate with the robot via RTDE
With the package comes a file called control_loop_configuration which contains all the recipes i.e. information from the robot we want to read.
image
I have worked using RTDE for a while now but only now I have noticed the following:
Whenever I try to read the TCP pose using RTDE in my python script, the values of the pose [x,y,z] correspond to those that appear on the polyscope. However, that is not the case with the orientations/ [Rx,Ry,Rz]

For example:
Values obtained using state.actual_TCP_pose: [1.2297, -2.40791 0.1764]
in polyscope: [1.622, 3.176, -0.233]

I know how to read and write registers and how to work with the robot but that seems like something internally in the robot is not right.
Any help?

@ajp @Ebbe

There are a number of topics on this in the forum. Basically there are two solutions for the rotations and for whatever reason Polyscope displays one and RTDE sends the other.

We noticed the same thing years ago. If you take your RTDE pose and enter it in the move screen you will go to back to the same point you were at when it was recorded by RTDE.

@mbush I looked at the old posts on the forum that describe the issue. The problem I have is that I am planning smooth trajectory which robot should follow but if the reading from the RTDE of the orientation are switched than I can’t suceed.
Did you found a solution to how to obtain orientation reading in axis-angle form consistently from RTDE?

I have checked and the reading from the polyscope is given in rotation vector form in [radians]. The recipe actual_TCP_pose should output values in axis_angles i.e. rotation vector.
Any other suggestions?

What RTDE is broadcasting is correct, it is just the other solution to the same pose. We record the positions (joint angle and pose) from RTDE and then use these to play back motion. That is how both our BotX and Cobot Welder solutions work for motion. You will get the motion that you expect. We also record joint positions so that we can use those as the qnear when using movej with a inverse kinematics soution.

Ok I got it finally,
Thank you for help everyone,

I guess its not needed to use the polyscope values in my situation. I should just stay consistent with the reading from the robot and that will work perfectly then.

Correct, as long as you are only using the RTDE values everything will work the way you want it to. Glad you got it working. I too sometimes find myself overthinking the issue.

Yeah,
For some reason I got confused that what I see on polyscope actually influences my python code, while it doesnt.
Thanks again :slight_smile:

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

@kaha thanks for that! We’ve been asking about that for years!

1 Like

@kaha
Thank you for an answer.
It would be beneficial to attach this information in URScript manual in the future releases or other place.
For a new person it can be very confusing.

I also found this very useful, I was seeing the same issue from the primary interface.