Feature rdx-val integration

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/ROS2SyncedRobotModel.java

@@ -27,6 +27,11 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node
       this(robotModel, ros2Node, robotModel.createFullRobotModel());
    }
 
+   public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node, boolean enforceUniqueReferenceFrames)
+   {
+      this(robotModel, ros2Node, robotModel.createFullRobotModel(enforceUniqueReferenceFrames));
+   }
+
    public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2NodeInterface ros2Node, FullHumanoidRobotModel fullRobotModel)
    {
       super(robotModel, fullRobotModel, robotModel.getHandModels(), robotModel.getSensorInformation());

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/RobotVersion.java

@@ -4,6 +4,11 @@
 
 public interface RobotVersion
 {
+   default boolean hasHead()
+   {
+      return true;
+   }
+
    default boolean hasArm(RobotSide robotSide)
    {
       return false;
@@ -14,11 +19,6 @@ default boolean hasBothArms()
       return hasArm(RobotSide.LEFT) && hasArm(RobotSide.RIGHT);
    }
 
-   default boolean hasHead()
-   {
-      return true;
-   }
-
    default boolean hasSakeGripperJoints(RobotSide side)
    {
       return false;

ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/virtualModelControl/VirtualModelControllerTestHelper.java

@@ -1934,6 +1934,12 @@ public MovingReferenceFrame getChestFrame()
       {
          return null;
       }
+
+      @Override
+      public MovingReferenceFrame getHeadFrame()
+      {
+         return null;
+      }
    }
 
    public static class ForcePointController implements RobotController

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXDualBlackflyProjection.java

@@ -81,17 +81,17 @@ public RDXDualBlackflyProjection(HumanoidReferenceFrames currentRobotFrames)
    {
       this.baseUI = RDXBaseUI.getInstance();
       this.robotZUpFrame = currentRobotFrames.getMidFootZUpGroundFrame();
-      robotCameraFrames.set(currentRobotFrames::getSituationalAwarenessCameraFrame);
+      robotCameraFrames.set(currentRobotFrames::getStereoCameraFrame);
 
       RigidBodyTransform stereoMidPoint = new RigidBodyTransform();
       MovingReferenceFrame chestFrame = currentRobotFrames.getChestFrame();
-      stereoMidPoint.interpolate(currentRobotFrames.getSituationalAwarenessCameraFrame(RobotSide.LEFT).getTransformToDesiredFrame(chestFrame),
-                                 currentRobotFrames.getSituationalAwarenessCameraFrame(RobotSide.RIGHT).getTransformToDesiredFrame(chestFrame),
+      stereoMidPoint.interpolate(currentRobotFrames.getStereoCameraFrame(RobotSide.LEFT).getTransformToDesiredFrame(chestFrame),
+                                 currentRobotFrames.getStereoCameraFrame(RobotSide.RIGHT).getTransformToDesiredFrame(chestFrame),
                                  0.5);
       stereoMidPointFrame = ReferenceFrameTools.constructFrameWithUnchangingTransformToParent("StereoMidPoint", chestFrame, stereoMidPoint);
       for (RobotSide side : RobotSide.values)
       {
-         RigidBodyTransform cameraToStereoMidPoint = currentRobotFrames.getSituationalAwarenessCameraFrame(side).getTransformToDesiredFrame(chestFrame);
+         RigidBodyTransform cameraToStereoMidPoint = currentRobotFrames.getStereoCameraFrame(side).getTransformToDesiredFrame(chestFrame);
          cameraToStereoMidPoint.invert();
          cameraToStereoMidPoint.multiply(stereoMidPoint);
          cameraToStereoMidPoint.invert();

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXSimulatedSensorFactory.java

@@ -180,7 +180,7 @@ public static RDXHighLevelDepthSensorSimulator createOusterLidar(ReferenceFrame
 
    public static RDXHighLevelDepthSensorSimulator createBlackflyFisheye(ROS2SyncedRobotModel syncedRobot)
    {
-      return createBlackflyFisheye(syncedRobot.getReferenceFrames().getSituationalAwarenessCameraFrame(RobotSide.RIGHT), syncedRobot::getTimestamp);
+      return createBlackflyFisheye(syncedRobot.getReferenceFrames().getStereoCameraFrame(RobotSide.RIGHT), syncedRobot::getTimestamp);
    }
 
    public static RDXHighLevelDepthSensorSimulator createBlackflyFisheyeImageOnlyNoComms(ReferenceFrame sensorFrame)
@@ -229,7 +229,7 @@ public static RDXHighLevelDepthSensorSimulator createChestRightBlackflyForObject
       double maxRange = 5.0;
       RDXHighLevelDepthSensorSimulator highLevelDepthSensorSimulator
             = new RDXHighLevelDepthSensorSimulator("Blackfly Right for Object Detection",
-                                                   referenceFrames.getSituationalAwarenessCameraFrame(RobotSide.RIGHT),
+                                                   referenceFrames.getStereoCameraFrame(RobotSide.RIGHT),
                                                    timeSupplier,
                                                    verticalFOV,
                                                    imageWidth,

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java

@@ -151,14 +151,6 @@ public void update(boolean interactablesEnabled)
          boolean desiredHandPoseChanged = false;
          for (RobotSide side : interactableHands.sides())
          {
-            if (syncedRobot.getHandWrenchCalculators().get(side) == null)
-               continue;
-
-            // wrench expressed in wrist pitch body fixed-frame
-            if (interactableHands.get(side).getEstimatedHandWrenchArrows().getShow() != showWrench)
-               interactableHands.get(side).getEstimatedHandWrenchArrows().setShow(showWrench);
-            interactableHands.get(side).updateEstimatedWrench(syncedRobot.getHandWrenchCalculators().get(side).getFilteredWrench());
-
             if (!interactableHands.get(side).isDeleted())
             {
                armIKSolvers.get(side).update(syncedRobot.getReferenceFrames().getChestFrame(), interactableHands.get(side).getControlReferenceFrame());
@@ -171,6 +163,14 @@ public void update(boolean interactablesEnabled)
                   desiredHandPoseChanged |= armIKSolvers.get(side).getDesiredHandControlPoseChanged();
                }
             }
+
+            if (syncedRobot.getHandWrenchCalculators().get(side) == null)
+               continue;
+
+            // wrench expressed in wrist pitch body fixed-frame
+            if (interactableHands.get(side).getEstimatedHandWrenchArrows().getShow() != showWrench)
+               interactableHands.get(side).getEstimatedHandWrenchArrows().setShow(showWrench);
+            interactableHands.get(side).updateEstimatedWrench(syncedRobot.getHandWrenchCalculators().get(side).getFilteredWrench());
          }
 
          // The following puts the solver on a thread as to not slow down the UI

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXInteractableHand.java

@@ -72,12 +72,15 @@ public RDXInteractableHand(RobotSide side,
       ForceSensorDefinition[] forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions();
       for (int i = 0; i < forceSensorDefinitions.length; i++)
       {
-         if (wristForceSensorNames.containsKey(side) && wristForceSensorNames.get(side).equals(forceSensorDefinitions[i].getSensorName()))
+         if (wristForceSensorNames != null)
          {
-            // wristWrenchArrows.put(side, new RDXSpatialVectorArrows(forceSensorDefinitions[i].getSensorFrame(), i));
-            sensorWristWrenchArrows = new RDXSpatialVectorArrows(forceSensorDefinitions[i].getSensorFrame(),
-                                                                 yoVariableClientHelper,
-                                                                 side.getLowerCaseName() + "WristSensor");
+            if (wristForceSensorNames.containsKey(side) && wristForceSensorNames.get(side).equals(forceSensorDefinitions[i].getSensorName()))
+            {
+               // wristWrenchArrows.put(side, new RDXSpatialVectorArrows(forceSensorDefinitions[i].getSensorFrame(), i));
+               sensorWristWrenchArrows = new RDXSpatialVectorArrows(forceSensorDefinitions[i].getSensorFrame(),
+                                                                    yoVariableClientHelper,
+                                                                    side.getLowerCaseName() + "WristSensor");
+            }
          }
       }
       ReferenceFrame afterLastWristJointFrame = fullRobotModel.getEndEffectorFrame(side, LimbName.ARM);

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXInteractableTools.java

@@ -20,7 +20,8 @@ public static String getModelFileName(RigidBodyDefinition rigidBodyDefinition)
       }
       if (modelFileGeometryDefinition == null || modelFileGeometryDefinition.getFileName() == null)
       {
-         LogTools.error("Interactables need a model file or implementation of shape visuals");
+         LogTools.error("Interactables for {} need a model file or implementation of shape visuals", rigidBodyDefinition.getName());
+         return null;
       }
       return modelFileGeometryDefinition.getFileName();
    }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXRobotCollidable.java

@@ -97,7 +97,7 @@ public RDXRobotCollidable(FrameShape3DReadOnly shape,
       linkFrame = syncedLinkFrame;
 
       RigidBodyTransform collisionToLinkFrameTransform = new RigidBodyTransform();
-      collisionShapeFrame = new MutableReferenceFrame("collisionShapeFrame" + rigidBodyName, linkFrame);
+      collisionShapeFrame = new MutableReferenceFrame("collisionShapeFrame" + rigidBodyName + collidableIndex, linkFrame);
 
       mouseCollidable = new MouseCollidable(this.shape);
       vrPickRayCollidable = new MouseCollidable(this.shape);