Universal Robots Forum

Move relative to Plane feature 2

I am trying to move relative to a plane feature. I have reviewed the content already at these links:

however I don’t think it gives me the complete answer. To summarise I wish to:

  1. Find the position relative to a plane feature. As already discussed this can be done with:

where p_1 is the position in the base coordinate system.
Plane_1 is the plane feature that I wish to reference.
p_2 is the point relative to Plane_1.

  1. I then wish to add offsets relative to the position in Plane_1.
    p_2_offset = p_2
    p_2_offset[0] = p_2_offset[0] + x_offset
    p_2_offset[1] = p_2_offset[1] + y_offset

  2. Now the main problem is how do I now translate this offset position back to the base coordinate system. Logically I thought I should try the following:

p_3 = pose_trans(pose_inv(p_2_offset),pose_inv(Plane_1))

where pose_inv(Plane_1) gives the position of the base coordinate system relative to Plane_1 coordinate system. I have tested the code and it doesn’t give the correct result. Does anyone have any ideas?