Hydroelastic spatial force visualization (RobotLocomotion#12378)

Adds the ability to visualize spatial forces (force and moment) to drake visualizer.

tools/workspace/drake_visualizer/plugin/show_hydroelastic_contact.py

@@ -12,6 +12,11 @@
 import drake as lcmdrakemsg
 
 from drake.tools.workspace.drake_visualizer.plugin import scoped_singleton_func
+from show_point_pair_contact import ContactVisModes
+
+
+# TODO(seancurtis-TRI) Make the dialog box for scaling force arrows in
+# show_point_pair_contact.py accessible to this plugin too.
 
 # TODO(seancurtis-TRI) Convert the modal dialog to a mode-less dialog to allow
 # continual tweaking of the visualization.
@@ -61,8 +66,9 @@ def get_mode_docstring(mode):
 class _ColorMapConfigurationDialog(QtGui.QDialog):
     '''A simple dialog for configuring the hydroelastic visualization'''
     def __init__(self, visualizer, show_contact_edges_state,
-                 show_pressure_state, max_pressure_observed,
-                 reset_max_pressure_observed_functor, parent=None):
+                 show_pressure_state, show_spatial_force_state,
+                 max_pressure_observed, reset_max_pressure_observed_functor,
+                 parent=None):
         QtGui.QDialog.__init__(self, parent)
         self.setWindowTitle('Hydroelastic contact visualization settings')
         self.reset_max_pressure_observed_functor = \
@@ -149,6 +155,16 @@ def __init__(self, visualizer, show_contact_edges_state,
         layout.addWidget(self.show_contact_edges, row, 1)
         row += 1
 
+        # Whether to show the force and moment vectors.
+        layout.addWidget(QtGui.QLabel('Render contact forces and moments'),
+                         row, 0)
+        self.show_spatial_force = QtGui.QCheckBox()
+        self.show_spatial_force.setChecked(show_spatial_force_state)
+        self.show_spatial_force.setToolTip('Renders the contact forces (in '
+                                           'red) and moments (in blue)')
+        layout.addWidget(self.show_spatial_force, row, 1)
+        row += 1
+
         # The maximum pressure value recorded and a button to reset it.
         self.pressure_value_label = QtGui.QLabel(
             'Maximum pressure value observed: {:.5e}'.format(
@@ -336,6 +352,12 @@ def __init__(self):
         self.show_contact_edges = True
         self.show_pressure = True
         self.max_pressure_observed = 0
+        self.show_spatial_force = True
+        # TODO(SeanCurtis-TRI): Make these variables settable through a dialog
+        #  box.
+        self.magnitude_mode = ContactVisModes.kFixedLength
+        self.global_scale = 0.3
+        self.min_magnitude = 1e-4
 
         menu_bar = applogic.getMainWindow().menuBar()
         plugin_menu = get_sub_menu_or_make(menu_bar, '&Plugins')
@@ -363,6 +385,7 @@ def configure_via_dialog(self):
         dlg = _ColorMapConfigurationDialog(self,
                                            self.show_contact_edges,
                                            self.show_pressure,
+                                           self.show_spatial_force,
                                            self.max_pressure_observed,
                                            self.reset_max_pressure_observed)
         if dlg.exec_() == QtGui.QDialog.Accepted:
@@ -372,6 +395,7 @@ def configure_via_dialog(self):
             self.min_pressure = float(dlg.min_pressure.text)
             self.max_pressure = float(dlg.max_pressure.text)
             self.show_contact_edges = dlg.show_contact_edges.isChecked()
+            self.show_spatial_force = dlg.show_spatial_force.isChecked()
             self.show_pressure = dlg.show_pressure.isChecked()
 
     def add_subscriber(self):
@@ -421,10 +445,81 @@ def handle_message(self, msg):
         # Set the color map.
         color_map = self.create_color_map()
 
+        # The scale value attributable to auto-scale.
+        auto_force_scale = 1.0
+        auto_moment_scale = 1.0
+        max_force = -1
+        max_moment = -1
+
+        # Determine the maximum force and moment magnitudes if the spatial
+        # forces are being visualized.
+        if self.show_spatial_force:
+            for surface in msg.hydroelastic_contacts:
+                force = np.array([surface.force_C_W[0],
+                                  surface.force_C_W[1],
+                                  surface.force_C_W[2]])
+                moment = np.array([surface.moment_C_W[0],
+                                   surface.moment_C_W[1],
+                                   surface.moment_C_W[2]])
+                force_mag = np.linalg.norm(force)
+                moment_mag = np.linalg.norm(moment)
+                if force_mag > max_force:
+                    max_force = force_mag
+                if moment_mag > max_moment:
+                    max_moment = moment_mag
+
+            # TODO(sean-curtis-TRI) Remove the following comment when this
+            # code can be exercised.
+            # The following code is not exercised presently because the
+            # magnitude mode is always set to kFixedLength.
+            if self.magnitude_mode == ContactVisModes.kAutoScale:
+                auto_force_scale = 1.0 / max_force
+                auto_moment_scale = 1.0 / max_force
+
         # TODO(drum) Consider exiting early if no visualization options are
         # enabled.
-        # Iterate over all triangles.
         for surface in msg.hydroelastic_contacts:
+            # Draw the spatial force.
+            if self.show_spatial_force:
+                point = np.array([surface.centroid_W[0],
+                                  surface.centroid_W[1],
+                                  surface.centroid_W[2]])
+                force = np.array([surface.force_C_W[0],
+                                  surface.force_C_W[1],
+                                  surface.force_C_W[2]])
+                moment = np.array([surface.moment_C_W[0],
+                                   surface.moment_C_W[1],
+                                   surface.moment_C_W[2]])
+                force_mag = np.linalg.norm(force)
+                moment_mag = np.linalg.norm(moment)
+
+                # Draw the force arrow if it's of sufficient magnitude.
+                if force_mag > self.min_magnitude:
+                    scale = self.global_scale
+                    if self.magnitude_mode == ContactVisModes.kFixedLength:
+                        # magnitude must be > 0 otherwise this force would be
+                        # skipped.
+                        scale /= force_mag
+
+                    d.addArrow(start=point,
+                               end=point + auto_force_scale * force * scale,
+                               tubeRadius=0.005,
+                               headRadius=0.01, color=[1, 0, 0])
+
+                # Draw the moment arrow if it's of sufficient magnitude.
+                if moment_mag > self.min_magnitude:
+                    scale = self.global_scale
+                    if self.magnitude_mode == ContactVisModes.kFixedLength:
+                        # magnitude must be > 0 otherwise this moment would be
+                        # skipped.
+                        scale /= moment_mag
+
+                    d.addArrow(start=point,
+                               end=point + auto_moment_scale * moment * scale,
+                               tubeRadius=0.005,
+                               headRadius=0.01, color=[0, 0, 1])
+
+            # Iterate over all triangles.
             for tri in surface.triangles:
                 va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]])
                 vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]])
@@ -493,7 +588,7 @@ def handle_message(self, msg):
 
             # Conditional necessary to keep DrakeVisualizer from spewing
             # messages to the console when the contact surface is empty.
-            if len(surface.triangles) > 0:
+            if len(msg.hydroelastic_contacts) > 0:
                 item.colorBy('RGB255')
 
 

tools/workspace/drake_visualizer/plugin/show_point_pair_contact.py

@@ -14,6 +14,8 @@
 from drake.tools.workspace.drake_visualizer.plugin import scoped_singleton_func
 
 
+# TODO(seancurtis-tri) Refactor this out of show_hydroelastic_contact.py and
+#                      show_point_pair_contact.py.
 class ContactVisModes:
     '''Common specification of contact visualization modes'''
     @staticmethod