IF condition fullfilled but not executed

Technical Questions URScript
1

Hello everyone,

i would like to compare poses in my if-condition but i cant get it working.

def mvtoPick():
    global speed_ms    = 1.750
    global speed_rads  = 1.750
    global accel_mss   = 1.200
    global accel_radss = 1.200
    global blend_radius_m = 0.001

    #Calculating Pose with Offset in Z-direction
    offset_var = pose_trans(pose_trans(setbase, setpose), p[0,0,-0.1,0,0,0])
    local target_pose = pose_trans(setbase,setpose)

    #Function to calculate a midpoint with z-offset to ensure a safe approach
    local Pose_Act = get_actual_tcp_pose() #Reads current TCP Pose
    if(not(Pose_Act == setbase) and  not(Pose_Act == offset_var) and not(Pose_Act == poseHome)):
        local dist = pose_dist(Pose_Act, target_pose)      #Calculates distance between the two points in m
        Pose_Mittelpunkt = pose_add(interpolate_pose(Pose_Act, target_pose, 0.5), p[0,0, dist, 0,0,0]) #Calculates Point inbetween two poses and adds an offset in z-Axis
        local dist_blend = pose_dist(Pose_Act, Pose_Mittelpunkt)/2 #Calcualtes value for blend radius
        movej(Pose_Mittelpunkt, 1, .5,0 , dist_blend)   #Moves to calculated point with calculated blend radius
    else:
        popup("Else funktion ausgefĂĽhrt", title="Popup #1",blocking=True)
    end
        
    #Move to offset
    movej(offset_var,accel_mss,speed_ms)
        
    #Move to Position
    movel(target_pose,accel_mss,speed_ms)
end

i start at the home position poseHome, which is predefined in the installation tab.
and immediatly get the act_tcp_pose but the condition is still met and executes the move.
What is the problem here?

2

Hi,

It is suggested to print the Pose_Act, then compare with the poseHome set in installation. Maybe it is not exactly equal, the diff of XYZ shall be small.

Thanks

3

What do you mean by printing? Is there a specific function for that or just save it in a variable and view it in the “Variables” Tab while running the problem?
Thanks for the help!

4

You can either set it to a variable or print it to the log with the textmsg(str1, str2) command.

5

OK i see the problem now.

When printing it to the log i get the pose p[0.17415,-0.6914,0.67685,-2.39577e-16,…]

the x, y and z coordinates are perfectly fine but for my rx, ry and rz coords i get those e-16 values.

So i need to round up the rotation coords for the if-condition to work correctly i assume?

6

You could round them yes.
There is the ceil() and floor() functions to round to closest integer. I don’t remember a function that can round to a certain decimal place.
You could also use pose_dist() or point_dist() which calculates a distance between two poses and you can then check whether the distance is within a certain threshold.

1 Like
7

ok i will try that aswell.

i tried writing a script that replaces values above ±1e-10 with 0.0, but its stuck in a loop somewhere.

is there any form of ide or plugin with which i can debug outside the polyscope environment?

i have the URScript Extension vor VS Code but i cant debug with it, are there any recommendations?

8

Thanks for the help!

Here is my final solution, where i get the current tcp and compare the distance.

def mvtoPick():
    global speed_ms    = 1.750
    global speed_rads  = 1.750
    global accel_mss   = 1.200
    global accel_radss = 1.200
    global blend_radius_m = 0.001

    #Calculating Pose with Offset in Z-direction
    offset_var = pose_trans(pose_trans(setbase, setpose), p[0,0,-0.1,0,0,0])
    local target_pose = pose_trans(setbase,setpose)

    #Function to calculate a midpoint with z-offset to ensure a safe approach
    local Pose_Act = get_actual_tcp_pose() #Reads current TCP Pose 
    if((pose_dist(setbase, Pose_Act) > 0.01)  and  (pose_dist(offset_var, Pose_Act) > 0.01)  and (pose_dist(poseHome, Pose_Act) > 0.01)): #If distance between current tcp and target greater than 10mm execute
        local dist = pose_dist(Pose_Act, target_pose)      #Calculates distance between the two points in m
        Pose_Mittelpunkt = pose_add(interpolate_pose(Pose_Act, target_pose, 0.5), p[0,0, dist, 0,0,0]) #Calculates Point inbetween two poses and adds an offset in z-Axis
        local dist_blend = pose_dist(Pose_Act, Pose_Mittelpunkt)/2 #Calcualtes value for blend radius
        movej(Pose_Mittelpunkt, 1, .5,0 , dist_blend)   #Moves to calculated point with calculated blend radius
    end
        
    #Move to offset
    movej(offset_var,accel_mss,speed_ms)
        
    #Move to Position
    movel(target_pose,accel_mss,speed_ms)
end
1 Like
9

Thank you for posting the solution.
Will be useful for others who run into this problem.
Comparing poses is a recurring challenge that is difficult to troubleshoot with the lack of debugging options available.

1 Like