Calculate relative position between two points

b_tetechs 2020-08-07

I'm trying to calculate the relative position between two points.

gauge (feature waypoint) = p[0.42155,0.06483,0.30493,-0.00004,0.00003,1.73676]

actual_tcp_pose = get_actual_tcp_pose() or get_actual_tool_flange_pose()

(in this case, p[0.23634,0.16249,0.26557,2.00009,-0.74194,2.05458])

Either way the result needs to be offset depending on one of the several TCP's stored.

actual_tcp_pose ---> gauge

p[0,0,0,0,0,0] ---> p[?,?,?,?,?,?]

The gripper in use is holding a cylinder where the gauge point is on the surface of, where the diameter is 90mm, and I've measured the offset to approx 215mm (give or take), which the result should be close to p[?,?,~0.215,?,?,?]

The x and y would theoretically be -45 to 45 with the 90mm diameter.

I've tried using pose_sub, pose_trans, pose_dist (need more then just distance). There doesn't appear to be a command/function available for this, unless I'm missing something. Has someone come across this and found a solution to calculate this?

The idea is to use 1 or 2 TCP's, change it to be the same position of the gauge, but in relation to the flange, and store/manipulate as needed.

jelms 2020-08-07

You are close. I think what you are looking for is described here in part c)

Poses are really relative to the world coordinates. So Pw_g is the pose of gauge in world coordinates. Pw_tcp is TCP in world. To get the pose of gauge relative to tcp, you need Ptcp_g. pose_trans() is a composition, so the Pw_g = Pw_tcp * Ptcp_g (ie pose_trans(Pw_tcp, Ptcp_g) ) and the tcp subscripts match up. Likewise Ptcp_g = Ptcp_w * Pw_g (ie pose_trans(Ptcp_w, Pw_g) ). Lastly pose_inv() switches the direction of the pose, so Ptcp_w is the location of the world (ie base) relative to the TCP and Ptcp_w = pose_inv(Pw_tcp).

Here is a script to demonstrate it. Values will show up on the log tab. check just applies the relative back to the tcp in world to confirm you get back to the Pw_g.

Pw_g = p[0.42155,0.06483,0.30493,-0.00004,0.00003,1.73676]
Pw_tcp = p[0.23634,0.16249,0.26557,2.00009,-0.74194,2.05458]
Ptcp_w = pose_inv(Pw_tcp)
Ptcp_g = pose_trans(Ptcp_w, Pw_g)
textmsg("relative ", Ptcp_g)
textmsg("check ", pose_trans(Pw_tcp, Ptcp_g))


Pw_g = p[0,0,3,0,0,3.14]
Pw_tcp = p[1,1,3,0,0,0]
Ptcp_w = pose_inv(Pw_tcp)
Ptcp_g = pose_trans(Ptcp_w, Pw_g)
textmsg("relative ", Ptcp_g)
textmsg("check ", pose_trans(Pw_tcp, Ptcp_g))

jelms 2020-08-07

Here is the output from running on the simulator

relative p[0.0443389,-0.0100236,0.208142,-0.569274,1.57328,-0.881222]
check p[0.42155,0.06483,0.30493,-4e-05,3e-05,1.73676]
relative p[-1,-1,0,0,0,3.14]
check p[0,0,3,0,0,3.14]

b_tetechs 2020-08-10

We're still testing the initial implementation of this, but so far it appears to be providing what's needed. Will update once we test further and involve the offset that'll be used on the tcp (rz) in question.