Align TCP to plane obtained with 3 points

General Application Help: How do I?
1

I have a UR20e and I need to perform some tasks with the TCP aligned to an unknown plane. First I am using the Direction command until Tool Contact to obtain 3 points of that plane and then generating it using the code from this post: Robot theory and how to use it in UR robots .

Once I have obtained the plane, I need to align the TCP of the arm to the plane. Polyscope has a built-in function on the Move Tab that aligns the TCP to the selected plane but I can’t find that function in URScript. Does anyone know if this function exists? If not, how could I replicate this function using URScript??

Thanks.

2

I’m not sure I am understanding your request.. but if you just move to coordinates that have 0 rotation component in that plane then the TCP will be aligned with it.

The align function in the move window is a little strange, it seems to maybe just align the z axis of the tool without doing full rotation alignment at times.. and sometimes it aligns it 180 now (which is useful, if it a little unpredictable).

1 Like
3

I need to align the Z axis of the Tool like the function on the Move window. I tried moving to coordinates that have 0 rotation components using the pose_trans function the following way:

my_plane = [X, Y, Z, Rx, Ry, Rz]

current_pose = get_actual_tcp_pose()

aligned_pose = pose_trans(current_pose, my_plane)

movej(aligned_pose)

After running this the robot just moves to a different position that is clearly not aligned with the plane and often has to be stopped as it can collide with itself.

Is my code is wrong, would you mind explaining why and what should I change? I am new to using arm robots and I feel like I am getting confused with some things.

Thanks.

4

Hi,

I dont think pose_trans is the correct thing you want. I suspect you want to just manually build your target.

take the x,y,z position from the current_pose.. and the Rx,Ry (and maybe Rz not sure if you want that) from your current position.

The robot stores targets as [x,y,z,Rx,Ry,Rz] with Rx,Ry,Rz forming a rotation vector (with magnitude of the rotation angle). I find these angles hard to work with intuitively if there is more than one rotation so I use the built in rpy2rotvec and rotvec2rpy functions quite a bit.

If you work in RPY (roll pitch yaw) to get the Z axis of the tool aligned with the Z of the plane you need to have the same RX and RY values (Rz can vary as this is rotation about the tool). This is I think close to what the align button in the move screen does. If you adjust the view in the move screen to show roll pitch yaw instead of rotation vectors you may be able to make more sense of what it’s doing.

so if you code something like this;

my_plane = [X, Y, Z, Rx, Ry, Rz]

current_pose = get_actual_tcp_pose()

#get rpy (roll pitch yaw) for current_pose

current_pose_rpy = rotvec2rpy([current_pose[3], current_pose[4], current_pose[5]]), 

#get rpy (roll pitch yaw) for my_plane

my_plane_rpy = rotvec2rpy([my_plane[3], my_plane[4], my_plane[5]])


#create target rotvec (rx,ry from my_plane, rz from current_pose.. not sure if this is what you want, but you can see the logic here)

target_rotvec = rpy2rotvec([my_plane_rpy[0], my_plane_rpy[1], current_pose_rpy[2]])

#make a pose with current_pose position (and my_plane rx,ry)
target_pose = p[current_pose[0], current_pose[1], current_pose[2],target_rotvec[0], target_rotvec[1], target_rotvec[2]]

I have just typed this in here so there might be syntax errors.. but you should be able to see the concept I use (convert rotation vectors to rpy so I can understand them and take rx and ry separately if needed, then build a pose with the individual parts you want).

Hope that helps somewhat… took me a while to get my head around this rotation vector thing (and can’t say I get it right 100% now anyway)

5

Hello pablo.

If you haven’t resolved it yet, please refer to the following method:

image
image1282×804 52.9 KB

A little explanation:

  1. Get the robot’s current TCP position in the BASE coordinate system via command “get_actual_tcp_pose()”
  2. Convert to TCP position in PLANE coordinate system. Sample script: current_pose_on_feature = pose_trans(pose_inv(Custom_Plane),current_pose)
  3. Align TCP with my_plane. Keep the position [X,Y,Z], only adjust the rotation vector [RX,RY,RZ] to [0,3,142,0].
  4. Convert the aligned TCP location in the plane to TCP in BASE (because the move command will use the BASE coordinate system).
  5. Move by script or command.
1 Like