Modelling screw fastening in drake - screw head reportedly comes into contact with end effector despite not being in contact

I am trying to model contact between a hex bit and a hex screw head in drake as part of a robotic fastening operation. However, I cannot get the screw head to get into the end effector: Screw is not in the end effector, yet somehow in contact.

I am using Drake's hydroelastic model for contact. My code is simply a torque controller that moves the arm to the screw's location and tries to fasten.

The SDF for the screw is:

collision_mesh_sdf = """<?xml version="1.0"?>
    <sdf version="1.7">
    <model name="screw">
        <pose>0 0 0 0 0 0</pose>
        <link name="screw">
        <collision name="collision">
            <geometry>
            <mesh>
                <scale>0.5 0.5 0.5</scale>
                <uri>file:///home/Hex_Screw.obj</uri>
            </mesh>
            </geometry>
            <drake:proximity_properties>
            <drake:compliant_hydroelastic/>
            <drake:hydroelastic_modulus>1e7</drake:hydroelastic_modulus>
            <drake:mu_dynamic>0.5</drake:mu_dynamic>
            <drake:hunt_crossley_dissipation>1.25</drake:hunt_crossley_dissipation>
            </drake:proximity_properties>
        </collision>
        <visual name="visual">
            <geometry>
            <mesh>
                <scale>0.5 0.5 0.5</scale>
                <uri>file:///home/Hex_Screw.obj</uri>
            </mesh>
            </geometry>
            <material>
            <diffuse>1.0 1.0 1.0 0.5</diffuse>
            </material>
        </visual>
        </link>
        <frame name="screw_frame">
        <pose relative_to="screw">0 0 -0.5 0 -1.57 0</pose>
        </frame>
    </model>
    </sdf>
    """

The SDF for the end effector is:

<link name="hex_bit">
  <pose>0 0 1.265 0 0 0</pose>
  <inertial>
    <pose>0 0 0.0 0 0 0</pose>
    <mass>.4</mass>
    <inertia>
      <ixx>0.0004</ixx>
      <ixy>0</ixy>
      <ixz>0</ixz>
      <iyy>0.0004</iyy>
      <iyz>0</iyz>
      <izz>0.0005</izz>
    </inertia>
  </inertial>
  <visual name="hex_bit_visual">
    <geometry>
      <mesh>
        <scale>1 1 1</scale>
        <uri>file:///home/HexBitEE.gltf</uri>
      </mesh>
    </geometry>
  </visual>
  <collision name="hex_bit_collision">
    <pose>0 0 0.0 1.57 0 0</pose>
    <geometry>
      <mesh>
        <scale>1 1 1</scale>
        <uri>file:///home/artyom/HexBitEE.obj</uri>
      </mesh>
    </geometry>
  </collision>
</link>

The scene graph properties are as follows:

scene_graph_config = scene_graph.get_config() scene_graph_config.default_proximity_properties.margin = 1e-6 scene_graph_config.default_proximity_properties.resolution_hint = 1e-6 scene_graph_config.default_proximity_properties.compliance_type = "compliant"

The multibody plant config (used for the robot model and screw) is:

config = MultibodyPlantConfig()
config.time_step = 1e-3
config.penetration_allowance = 1e-3
config.contact_model = "hydroelastic"
config.discrete_contact_approximation = "lagged"

Could you please give me any suggestions as for why my screw seems to come in contact with my end effector even when not being in contact? Thank you for your help!

You need to take some extra steps to model your screw and bit. This all comes down to appropriate representations of hydroelastic contact.

1. You should make sure both meshes (screw and bit) have explicit hydroelastic declarations. Both can be compliant, or one can be rigid and the other compliant. Either should work. Right now, your screw is declared to be compliant (see the next on that topic), but your hex bit has no such declaration.
2. When you declare a mesh collision geometry as compliant hydroelastic but provide a surface mesh (i.e., .obj file), the convex hull of that mesh will be used. What you really need to do is create a tetrahedral mesh version of that geometry.

We've got a document for providing guidelines on how to turn your .obj into a .vtk appropriate for compliant contact. You can read it here.