I am developing a teleoperation system for my UR5e, where the user manually guides a smaller “leader” arm while a controller is designed to make the UR5e follow the same motions. I have started using the new joint torque control interface to do so, because using joint torques allows the robot to make low-impact contact with the environment without entering a protective stop due to path deviation.
My approach is to use a PD controller on joint position and velocity errors to generate the motor torque commands. I am using the ur_rtde package in Python to communicate with the arm. The logic is roughly:
Kp = np.array([200, 150, 150, 25, 25, 20])
Kd = 0.2 * Kd
# this runs in a 500Hz control loop
# qd and vd are desired joint position and velocity, q and v are the actual values
torque_cmd = Kp * (qd - q) + Kd * (vd - v)
# send using directTorque...
After some tuning of the gains, this works quite well. The only problem we’ve run into is that there appears to be a small “deadzone” around the origin, such that small enough torques produce no movement of the arm. This means that the above controller has some steady-state joint position error. Whether the friction_comp argument of directTorque is true or false does not appear to make too much difference.
Has anyone run into this problem? What have you done about it? One possibility is to add an integral term to the controller to eliminate the steady-state error, but this requires even more tuning and concerns about stability, etc.