Universal Robots Forum

Measurement of forces - movel

Hi all,
@ajp
I am trying to write a code that initiates a robot above a surface and then slowly descends till reaching contact until a force in the z-direction reaches 2N, which causes the loop to stop.

The issue I am having is, that once i initialize movel the loop is not going to continue running, instead it waits till movel finished its task and only then allows the loop to continue.
Any solutions?

How to let the loop which collects data from sensor run, while in the same time let the robot move to the desired location?

Here is my code

F_start = FT_sensor.force_moment_feedback().reshape(6, 1)
dist = 0.01
start = np.array([0.0000, -0.6732,  0.15768,   -0.0425,  3.1233,  0.0983])
end = np.array([0.0000, -0.6732,  0.1483-dist,   -0.0425,  3.1233,  0.0983])
robot.movel(start)
j = 0
fx = []
fy = []
fz = []
while True:

    print(FT_sensor.force_moment_feedback().reshape(6, 1)[2]-F_start[2])
    j += 1
    real_force = FT_sensor.force_moment_feedback().reshape(6, 1)-F_start
    fx.append(real_force[0])
    fy.append(real_force[1])
    fz.append(real_force[2])

    robot.movel(end, a=0.05, v=0.002)

    if np.abs(FT_sensor.force_moment_feedback().reshape(6, 1)[2]-F_start[2]) > 2.0:
        robot.stopl()
        print(np.abs(FT_sensor.force_moment_feedback().reshape(6, 1)[2]-F_start[2]))
        print("inner loop broken")
        break

robot.movel(start)

Help will be greatly appreciated :slight_smile:

There’s an option under while and if statements in Polyscope called “check expression continuously”, which creates a thread to monitor the value (in this case z force) while the loop is running. I’d make a simple program in polyscope using it then look at how it works in the polyscope generated script program to see how it works.

1 Like