Feature rdx-generalization-other-robots

atlas/src/main/resources/us/ihmc/atlas/parameters/AtlasFootstepPlannerParameters.json

@@ -1,5 +1,6 @@
 {
   "title" : "Atlas footstep planner parameters",
+  "Use GPU" : true,
   "AStar heuristics weight" : 1.5,
   "Max branch factor" : 175,
   "Enable expansion mask" : true,

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

@@ -26,6 +26,11 @@ public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2Node ros2Node)
    {
       this(robotModel, ros2Node, robotModel.createFullRobotModel());
    }
+
+   public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2Node ros2Node, boolean enforceUniqueReferenceFrames)
+   {
+      this(robotModel, ros2Node, robotModel.createFullRobotModel(enforceUniqueReferenceFrames));
+   }
 
    public ROS2SyncedRobotModel(DRCRobotModel robotModel, ROS2Node ros2Node, FullHumanoidRobotModel fullRobotModel)
    {

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-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlanningModule.java

@@ -60,12 +60,13 @@ public class FootstepPlanningModule implements CloseableAndDisposable
    private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters;
 
    private final WaypointDefinedBodyPathPlanHolder bodyPathPlanHolder = new WaypointDefinedBodyPathPlanHolder();
-   private final AStarBodyPathPlannerInterface bodyPathPlannerInterface;
+   private AStarBodyPathPlannerInterface bodyPathPlannerInterface;
 
    // TODO plan then snap planner needs to work for height maps.
    private final PlanThenSnapPlanner planThenSnapPlanner;
    private final AStarFootstepPlanner aStarFootstepPlanner;
    private final List<VariableDescriptor> footstepPlanVariableDescriptors;
+   private final SideDependentList<ConvexPolygon2D> footPolygons;
 
    private final AtomicBoolean isPlanning = new AtomicBoolean();
    private final FootstepPlannerRequest request = new FootstepPlannerRequest();
@@ -81,7 +82,7 @@ public class FootstepPlanningModule implements CloseableAndDisposable
    private final List<Consumer<SwingPlannerType>> swingReplanRequestCallbacks = new ArrayList<>();
    private final List<Consumer<FootstepPlan>> swingReplanStatusCallbacks = new ArrayList<>();
 
-   private final boolean useGPU;
+   private boolean useGPU;
 
    public FootstepPlanningModule()
    {
@@ -99,6 +100,11 @@ public FootstepPlanningModule(String name)
            null);
    }
 
+   public FootstepPlanningModule(boolean useGPU)
+   {
+      this(FootstepPlanningModule.class.getSimpleName(), useGPU);
+   }
+
    public FootstepPlanningModule(String name, boolean useGPU)
    {
       this(name,
@@ -141,8 +147,10 @@ public FootstepPlanningModule(String name,
       this.name = name;
       this.aStarBodyPathPlannerParameters = aStarBodyPathPlannerParameters;
       this.footstepPlannerParameters = footstepPlannerParameters;
+      this.footPolygons = footPolygons;
 
       this.useGPU = useGPU;
+
       if (useGPU)
       {
          this.bodyPathPlannerInterface = new GPUAStarBodyPathPlanner(footstepPlannerParameters, aStarBodyPathPlannerParameters, footPolygons, stopwatch);
@@ -210,6 +218,20 @@ public void destroy()
          ((GPUAStarBodyPathPlanner) bodyPathPlannerInterface).destroyOpenCLStuff();
    }
 
+   public void enableGPUBodyPathPlanner(boolean useGPU)
+   {
+      if (this.useGPU && !useGPU)
+      {
+         destroy();
+         bodyPathPlannerInterface = new AStarBodyPathPlanner(footstepPlannerParameters, aStarBodyPathPlannerParameters, footPolygons, stopwatch);
+      }
+      else if (!this.useGPU && useGPU)
+      {
+         bodyPathPlannerInterface = new GPUAStarBodyPathPlanner(footstepPlannerParameters, aStarBodyPathPlannerParameters, footPolygons, stopwatch);
+      }
+      this.useGPU = useGPU;
+   }
+
    private void handleRequestInternal(FootstepPlannerRequest request) throws Exception
    {
       this.request.set(request);

ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java

@@ -10,6 +10,7 @@ public class DefaultFootstepPlannerParameters extends StoredPropertySet implemen
 {
    public static final StoredPropertyKeyList keys = new StoredPropertyKeyList();
 
+   public static final BooleanStoredPropertyKey useGPU = keys.addBooleanKey("Use GPU");
    public static final DoubleStoredPropertyKey astarHeuristicsWeight = keys.addDoubleKey("AStar heuristics weight");
    public static final IntegerStoredPropertyKey maxBranchFactor = keys.addIntegerKey("Max branch factor");
    public static final BooleanStoredPropertyKey enableExpansionMask = keys.addBooleanKey("Enable expansion mask");

ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParametersBasics.java

@@ -10,6 +10,11 @@ default void set(DefaultFootstepPlannerParametersReadOnly footstepPlannerParamet
       setAll(footstepPlannerParameters.getAll());
    }
 
+   default void setUseGPU(boolean useGPU)
+   {
+      set(DefaultFootstepPlannerParameters.useGPU, useGPU);
+   }
+
    default void setAStarHeuristicsWeight(double aStarHeuristicsWeight)
    {
       set(DefaultFootstepPlannerParameters.astarHeuristicsWeight, aStarHeuristicsWeight);
@@ -343,6 +348,7 @@ default void setCliffHeightThreshold(double cliffHeightThreshold)
    default void set(FootstepPlannerParametersPacket parametersPacket)
    {
       double noValue = FootstepPlannerParametersPacket.DEFAULT_NO_VALUE;
+      setUseGPU(parametersPacket.getUseGpu());
       setCheckForBodyBoxCollisions(parametersPacket.getCheckForBodyBoxCollisions());
       setCheckForPathCollisions(parametersPacket.getCheckForPathCollisions());
       setIntermediateBodyBoxChecks((int) parametersPacket.getIntermediateBodyBoxChecks());

ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParametersReadOnly.java

@@ -14,6 +14,13 @@ public interface DefaultFootstepPlannerParametersReadOnly extends StoredProperty
    ///////////////////////////////////////////////////////////////////////////////////////////////////
    /////////////////////////////         Algorithm parameters       //////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////
+   /**
+    * Whether we use the GPU for body path planning
+    */
+   default boolean getUseGPU()
+   {
+      return get(useGPU);
+   }
 
    /**
     * Heuristic inflation weight, used to speed up the planner at the expense of optimality
@@ -635,6 +642,7 @@ default FootstepPlannerParametersPacket getAsPacket()
    {
       FootstepPlannerParametersPacket packet = new FootstepPlannerParametersPacket();
 
+      packet.setUseGpu(getUseGPU());
       packet.setAStarHeuristicsWeight(getAStarHeuristicsWeight());
       packet.setMaxBranchFactor(getMaxBranchFactor());
       packet.setEnableExpansionMask(getEnableExpansionMask());

ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/tools/FootstepPlannerMessageTools.java

@@ -50,6 +50,7 @@ public static void copyParametersToPacket(FootstepPlannerParametersPacket packet
          return;
       }
 
+      packet.setUseGpu(parameters.getUseGPU());
       packet.setReferencePlanAlpha(parameters.getReferencePlanAlpha());
       packet.setCheckForBodyBoxCollisions(parameters.getCheckForBodyBoxCollisions());
       packet.setCheckForPathCollisions(parameters.getCheckForPathCollisions());

ihmc-footstep-planning/src/main/resources/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.json

@@ -1,5 +1,6 @@
 {
   "title" : "DefaultFootstepPlannerParameters",
+  "Use GPU" : true,
   "AStar heuristics weight" : 1.5,
   "Max branch factor" : -1,
   "Enable expansion mask" : true,

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

@@ -179,7 +179,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)
@@ -226,7 +226,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,
@@ -241,8 +241,12 @@ public static RDXHighLevelDepthSensorSimulator createChestRightBlackflyForObject
       return highLevelDepthSensorSimulator;
    }
 
-   public static RDXHighLevelDepthSensorSimulator createChestZED2ForObjectDetection(HumanoidReferenceFrames referenceFrames,
-                                                                                    LongSupplier timeSupplier)
+   public static RDXHighLevelDepthSensorSimulator createChestZED2ForObjectDetection(ROS2SyncedRobotModel syncedRobot)
+   {
+      return createChestZED2ForObjectDetection(syncedRobot.getReferenceFrames(), syncedRobot::getTimestamp);
+   }
+
+   public static RDXHighLevelDepthSensorSimulator createChestZED2ForObjectDetection(HumanoidReferenceFrames referenceFrames, LongSupplier timeSupplier)
    {
       double publishRateHz = 20.0;
       double verticalFOV = 70.0;

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);

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXHandPoseAction.java

@@ -361,7 +361,7 @@ protected void renderImGuiWidgetsInternal()
             ImGui.popItemWidth();
             if (ImGui.button(labels.get("Set Configuration to Synced Arm")))
             {
-               for (int i = 0; i < getDefinition().getJointAngles().getLength(); i++)
+               for (int i = 0; i < armJointNames.length; i++)
                {
                   OneDoFJointBasics syncedJoint = syncedRobot.getFullRobotModel().getArmJoint(getDefinition().getSide(), armJointNames[i]);
                   if (syncedJoint != null)