Universal Robots Forum

How to find the distance of the tcp movement from one wavepoint to another?

Is there any way to find out the distance between two-waypoints?

For example, I’m doing glue application and using the code exactly how it’s mentioned here: Universal Robots - Modify robot trajectory by overlaying custom motion

The program works good but I want to know the length of the sine wave. Could any one of you know how to find it? If not distance, do you know how to calculate the speed of the sine wave? Please let me know if you have any ideas. I’m attaching some photos for reference.


Hello @p.honnegowda ,

the general Script Function for calculating the distance between two poses is pose_dist( <…> ).
You can use it to sum up each distance between the separate poses you calculate.

Would this be applicable in your scenario?

1 Like

Hello @sko,

Thanks for your response. Actually, I tried this script before it just gives the distance between the two waypoints without rotation (it works similar to point_dist(p1,p2). I don’t know why!!!

See this picture

I want something like this.

Kind regards,


Hi @p.honnegowda,

there is not direct script function available to calculate the distance here. My recommendation would be to create thread, which gets the actual_tcp_pose(), waits a set amount of ms and then reads out the current pose again. You can then use pose_dist() or point_dist() to calculate this minimum step and sum it up over the whole movement.

Depending on your movement speed the needed wait time can vary.

1 Like

Instead of waiting a certain time between capturing the actual TCP pose in the thread, could you instead monitor the offset variable? Capture the position at one pre-determined point in the sine wave cycle… and then when the offset equals that value again you can assume you’ve completed half a cycle… (or a whole cycle if it’s the min or max value) then calculate the actual distance between those two waypoints?


Thank you both of you @sko @ajp

I am using the thread as it is in the image it gives the length of a complete cycle including other movements. But if I want to get the length in between how do I do it?

You could set a boolean variable high before the move and set it low after the move. Use the variable to start and stop your distance measurement…

Or create an event triggered by the variable

1 Like

Can you add an if statement in your main offset calculation code to capture the TCP pose when the offset is equal to a certain value?

Below code not tested but should record the tcp pose the first 3 times the offset is zero… the distance between the first and third values should give you the wavelength?

capture = 0
blank = p[[0,0,0,0,0,0]
values = [blank, blank, blank]
i = 0
if (offset[0] == capture and i < length(values) ):
    values[i] = get_actual_tcp_pose()
    i = i+1
1 Like