Modeling Backlash

Imagine you have a motor turning a gear which turns another gear. Unless those gears are perfect (pro tip: they aren’t), you are going to have something called backlash.

Backlash Explained

Backlash is the looseness that occurs when gear systems are not perfect – there are gaps between the teeth. The end result is that when gear A changes direction, gear B will not reflect that change in direction right away.

backlash diagram

You can measure the amount of backlash in your system in radians (or degrees if you swing that way). Turn gear A clockwise for a bit, then start to turn counterclockwise and measure how far gear A gets before gear B starts to move in the same direction as gear A.

Backlash in the Beaker Robot

My balancing robot robot Beaker has bevel gears, and you can see the backlash very clearly:
backlash in beaker

In the gif above you can see that the wheel turns even though the motor bevel gear turns just a bit. Also I can tell you that given the wheel turning in the gif, the motor’s rotor is not turning at all — its easy to feel because there would be more resistance when turning the wheel.

I measured and there is 0.1512 rads (or 8.663 degrees) of backlash!

Why Model Backlash

We can now talk about modeling backlash. First of all — do we need to? Of course it depends on why you are modeling. In my case, I’m experimenting with transfer learning – I’m training a neural net in virtual, then taking the trained net and applying it to the real Beaker robot. You know, like Neo…

enter image description here

So far, the transfer to the real robot has not been successful, because the model is not realistic enough. I believe modeling backlash will help.

How to Model Backlash

I’m using the awesome Bullet Physics Engine, so I took a look around to see if there was anything that facilitated modeling backlash directly, and there was not. Turns out there is a pretty easy solution, and I’m going to describe it here.

Backlash Model Demo

Notice that there is backlash between the green and orange disks. The motor (blue) is free to turn the green disk as much as it likes in either direction. The orange disk will follow, after a bit of backlash.

simulated backlash

Method of Modeling Backlash

In the demo above, there are only two joints: the joint connecting the actuator (blue) to the lower disk (green), and the joint connecting the lower disk (green) to the upper disk (orange).

The blue-green joint is continuous – it can rotate as many times as we like. The green-orange joint is NOT continuous, and has a 0.1512 rad constraint on motion.

There are two files, and I’ll just embed them since they are pretty small. The first is the URDF file which describes the model, and the second is the python script which plays out the simulation.

<?xml version="1.0"?>
<robot name="bot">

  <!-- colors ................................................................. -->
  <material name="red">
    <color rgba="0.8 0 0 1"/>
  </material>

 <material name="green">
    <color rgba="0 0.8 0 0.75"/>
  </material>

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="orange">
    <color rgba="0.8 0.4 0 0.75"/>
  </material>

  <!-- cube base .................................................................-->
  <link name="body">
    <visual>
      <geometry>
        <box size="1 1 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" />
      <material name="blue"/>
    </visual>

    <collision>
      <geometry>
        <box size="1 1 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" />
    </collision>
  </link>

  <!-- diskBottom .................................................................-->
  <link name="diskBottom">
    <visual>
      <geometry>
        <cylinder length="0.10" radius="0.4"/>
      </geometry>
      <origin rpy="0 1.5708 0"/>
      <material name="green"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.10" radius="0.4"/>
      </geometry>
      <origin rpy="0 1.5708 0"/>
    </collision>
  </link>
  <joint name="diskToBox" type="continuous">
    <origin xyz="0 0 0.55" rpy="0 1.5708 0"/>
    <parent link="body"/>
    <child link="diskBottom"/>
  </joint>

  <!-- indicatorBottom .................................................................-->
  <link name="indicatorBottom">
    <visual>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <joint name="indicatorToDiskBottom" type="fixed">
    <origin xyz="0 0.4 0" rpy="1.5708 0 0"/>
    <parent link="diskBottom"/>
    <child link="indicatorBottom"/>
  </joint>

  <!-- diskTop .................................................................-->
  <link name="diskTop">
    <visual>
      <geometry>
        <cylinder length="0.10" radius="0.4"/>
      </geometry>
      <origin rpy="0 1.5708 0"/>
      <material name="orange"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.10" radius="0.4"/>
      </geometry>
      <origin rpy="0 1.5708 0"/>
    </collision>
  </link>
    <!-- the type revolute with limits is the crucial element. Upper - lower = 0.1512 rads -->
  <joint name="diskToDisk" type="revolute">
    <limit lower="-0.0756" upper="0.0756" />
    <origin xyz="-0.15 0 0"/>
    <parent link="diskBottom"/>
    <child link="diskTop"/>
  </joint>

  <!-- indicatorTop .................................................................-->
  <link name="indicatorTop">
    <visual>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <joint name="indicatorToDiskTop" type="fixed">
    <origin xyz="0 .40 0"/>
    <parent link="diskTop"/>
    <child link="indicatorTop"/>
  </joint>

</robot>

And now to see it in action, we have the following python script:

import pybullet as p
import pybullet_data
import time


physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.8)
planeId = p.loadURDF("plane.urdf")
gearBoxId = p.loadURDF("gearbox.urdf")


numJoints = p.getNumJoints(gearBoxId)
print("Number of joints: {}".format(numJoints))

joints = {}
for i in range(0, numJoints):
    info = p.getJointInfo(gearBoxId, i)
    if(str(info[1]) == "b'diskToBox'"):
            joints['diskToBox'] = info
    if(str(info[1]) == "b'diskToDisk'"):
            joints['diskToDisk'] = info

def setBottomVelocity(vel):
    p.setJointMotorControl2(bodyUniqueId=gearBoxId, 
        jointIndex=joints['diskToBox'][0], 
        controlMode=p.VELOCITY_CONTROL,
        targetVelocity = vel,
        force = 10)

# setting the force=0 is important because rotational joints have "active" motors, meaning they will hold their position (unless you set force = 0).
p.setJointMotorControl2(bodyUniqueId=gearBoxId, 
    jointIndex=joints['diskToDisk'][0], 
    controlMode=p.VELOCITY_CONTROL,
    force = 0)

# These lines just place the camera at a good place to see the action
distance = 2
yaw = 145
height = -30
p.resetDebugVisualizerCamera(distance, yaw, height, [0,0,0])

dir = 1
setBottomVelocity(dir)

i = 0
while True:
        i += 1
        if(i > 250):
            dir *= -1
            i = 0
            setBottomVelocity(dir)

        p.stepSimulation()
        time.sleep(.001)

p.disconnect()