Freedrive mode with Servoj in same URscript problem

Kasem 2020-02-19

Hello

I'm working on controlling UR3e SW V5.6 over Profinet , i want to execute trajectories by sending the joints positions and control free drive too based on if conditions dependent on bit registers , as shown in the URscript below , but i have a problem that the robot vibrates if both variables are false and if i touched the robot , the trajectory execution works and free drive works too , but i think the problem is in the else part , i want to end free drive mode and go to default positions ,this is where the robot vibrates

def joints_move_register_v3():

Exceute_trajectory=read_input_boolean_register(3)

Exceute_Teach_Mode=read_input_boolean_register(4)

if (Exceute_trajectory == True and Exceute_Teach_Mode == False ):

q0=read_input_float_register(0)

q1=read_input_float_register(1)

q2=read_input_float_register(2)

q3=read_input_float_register(3)

q4=read_input_float_register(4)

q5=read_input_float_register(5)

q=[0.0,-1.57,q2,-1.57,q4,q5]

servoj(q,0,0,0.008,0.1,300)

elif(Exceute_Teach_Mode == True and Exceute_trajectory == False ):

freedrive mode()

else:

end_freedrive_mode()

q=[0.0,-1.57,0.0,-1.57,0.0,0.0]

servoj(q,0,0,0.008,0.1,300)

end

end

if someone who have good URscript programming skills please help me to make it work

matthewd92 2020-02-19

The first thing I notice is that your time slice is wrong for a e-series, it should be 0.002 as that’s the clock cycle of the e-series (500 Hz), cb series is 0.008(125 Hz).

Is your PLC setting the motion profile every 2ms as well? It will need to control the robot at that frequency for motion to be smooth.

jelms 2020-02-26

I think it may be that you are always issuing servoj commands and not issuing a stopj. I'd check if it's already at the position within some tolerance

err_tolerance = 1e-3
ps = get_forward_kin(q)
pc = get_forward_kin() # get actual (could use get_actual_tcp_pose)
dist = point_dist(pc, ps)
if dist > err_tolerance:
    servoj(q,0,0,0.008,0.1,300)
else:
    stopj(2)
end

Or if you want to use joint norm

# helper function to return a q that is the q1 - q2
def q_diff(q1, q2):
    ii = 0
    while ii < 6:
        q1[ii] = q1[ii] - q2[ii]
        ii = ii + 1
    end
    return q1
end

if  norm(q_diff(q, get_actual_joint_positions())) > err_tolerance:
    servoj(q,0,0,0.008,0.1,300)
else:
    stopj(2)
end