Freedrive mode with Servoj in same URscript problem

Kasem

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

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

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)<br>if dist > err_tolerance:
<div>    servoj(q,0,0,0.008,0.1,300)
else:
    stopj(2)
end

</div>

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