Fix VR bugs.

Author: Luigi Penco

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXHandConfigurationManager.java

@@ -73,30 +73,37 @@ public void renderImGuiWidgets()
 
    public void publishHandCommand(RobotSide side, @Nullable SakeHandPreset handPreset, boolean calibrate, boolean reset)
    {
-      SakeHandDesiredCommandMessage sakeHandDesiredCommandMessage = new SakeHandDesiredCommandMessage();
-      sakeHandDesiredCommandMessage.setRobotSide(side.toByte());
-      SakeHandParameters.resetDesiredCommandMessage(sakeHandDesiredCommandMessage);
-
-      if (calibrate)
-      {
-         sakeHandDesiredCommandMessage.setRequestCalibration(true);
-      }
-      else if (reset)
+      if (sakeHandStatus.size() == 2)
       {
-         sakeHandDesiredCommandMessage.setRequestResetErrors(true);
+         SakeHandDesiredCommandMessage sakeHandDesiredCommandMessage = new SakeHandDesiredCommandMessage();
+         sakeHandDesiredCommandMessage.setRobotSide(side.toByte());
+         SakeHandParameters.resetDesiredCommandMessage(sakeHandDesiredCommandMessage);
+
+         if (calibrate)
+         {
+            sakeHandDesiredCommandMessage.setRequestCalibration(true);
+         }
+         else if (reset)
+         {
+            sakeHandDesiredCommandMessage.setRequestResetErrors(true);
+         }
+         else if (handPreset != null)
+         {
+            double handPositionLowerLimit = sakeHandStatus.get(side).getPositionLowerLimit();
+            double handPositionUpperLimit = sakeHandStatus.get(side).getPositionUpperLimit();
+
+            sakeHandDesiredCommandMessage.setGripperDesiredPosition(SakeHandParameters.handOpenAngleToPosition(handPreset.getHandOpenAngle(),
+                                                                                                               handPositionLowerLimit,
+                                                                                                               handPositionUpperLimit));
+            sakeHandDesiredCommandMessage.setRawGripperTorqueLimit(SakeHandParameters.gripForceToRawTorque(handPreset.getFingertipGripForceLimit()));
+         }
+
+         RDXBaseUI.pushNotification("Commanding hand configuration...");
+         communicationHelper.publish(SakeHandAPI.getHandSakeCommandTopic(robotName, side), sakeHandDesiredCommandMessage);
       }
-      else if (handPreset != null)
+      else
       {
-         double handPositionLowerLimit = sakeHandStatus.get(side).getPositionLowerLimit();
-         double handPositionUpperLimit = sakeHandStatus.get(side).getPositionUpperLimit();
-
-         sakeHandDesiredCommandMessage.setGripperDesiredPosition(SakeHandParameters.handOpenAngleToPosition(handPreset.getHandOpenAngle(),
-                                                                                                            handPositionLowerLimit,
-                                                                                                            handPositionUpperLimit));
-         sakeHandDesiredCommandMessage.setRawGripperTorqueLimit(SakeHandParameters.gripForceToRawTorque(handPreset.getFingertipGripForceLimit()));
+         RDXBaseUI.pushNotification("No hands on this robot.");
       }
-
-      RDXBaseUI.pushNotification("Commanding hand configuration...");
-      communicationHelper.publish(SakeHandAPI.getHandSakeCommandTopic(robotName, side), sakeHandDesiredCommandMessage);
    }
 }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeManager.java

@@ -264,13 +264,13 @@ private void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool
             }
             case JOYSTICK_WALKING -> joystickBasedStepping.getRenderables(renderables, pool);
          }
-      }
 
-      if (stereoVision.isEnabled())
-         stereoVision.getDualBlackflySphericalProjection().getRenderables(renderables, pool, sceneLevels);
+         if (stereoVision.isEnabled())
+            stereoVision.getDualBlackflySphericalProjection().getRenderables(renderables, pool, sceneLevels);
 
-      if (vrModeControls.getRenderOnLeftHand().get())
-         vrModeControls3DPanel.getRenderables(renderables, pool);
+         if (vrModeControls.getRenderOnLeftHand().get())
+            vrModeControls3DPanel.getRenderables(renderables, pool);
+      }
    }
 
    public void destroy()