Fix behavior footstep plan action bug and document graphic classes. (#638)

Author: calvertdw

ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTBidirectionalRigidBodyTransform.java

@@ -35,6 +35,18 @@ public RigidBodyTransform getValueUnsafe()
       return getValueInternal();
    }
 
+   /** Prefer this method in the case you need to call it every tick, as it no-ops in the case the value is the same. */
+   public void setValue(RigidBodyTransformReadOnly value, double epsilon)
+   {
+      // rotation and translation must be checked separately to handle pose-transform comparison case
+      // translation must use epsilonEquals because it can be vector vs. point
+      if (!(getValueInternal().getRotation().geometricallyEquals(value.getRotation(), epsilon)
+         && getValueInternal().getTranslation().epsilonEquals(value.getTranslation(), epsilon)))
+      {
+         getValueAndModify().set(value);
+      }
+   }
+
    public void toMessage(Pose3D poseMessage)
    {
       poseMessage.set(getValueReadOnly());

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

@@ -37,7 +37,6 @@
 import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode;
 import us.ihmc.rdx.ui.behavior.tools.RDXCRDTTools;
 import us.ihmc.rdx.ui.gizmo.RDXSelectablePose3DGizmo;
-import us.ihmc.rdx.ui.graphics.RDXFootstepGraphic;
 import us.ihmc.rdx.ui.graphics.RDXFootstepPlanGraphic;
 import us.ihmc.rdx.ui.widgets.ImGuiFootstepsWidget;
 import us.ihmc.rdx.vr.RDXVRContext;
@@ -73,8 +72,7 @@ public class RDXFootstepPlanAction extends RDXActionNode<FootstepPlanActionState
    private final RDXMutableArrowModel goalArrowGraphic = new RDXMutableArrowModel();
    private final RDXSelectablePose3DGizmo goalStancePointGizmo = new RDXSelectablePose3DGizmo();
    private final RDXSelectablePose3DGizmo goalFocalPointGizmo = new RDXSelectablePose3DGizmo();
-   private final SideDependentList<RDXFootstepGraphic> goalFeetGraphics = new SideDependentList<>();
-   private final SideDependentList<RDXSelectablePose3DGizmo> goalFeetGizmos = new SideDependentList<>();
+   private final SideDependentList<RDXFootstepPlanActionGoalFootstep> goalFeet = new SideDependentList<>();
    private final RDX3DPanelTooltip tooltip;
    private final ImGuiFootstepsWidget footstepsWidget = new ImGuiFootstepsWidget();
    private final RDXFootstepPlanGraphic previewFootstepPlan;
@@ -165,16 +163,7 @@ public RDXFootstepPlanAction(long id,
       goalFocalPointGizmo.getPoseGizmo().getCenterSphereToTorusRatio().set(0.5f);
 
       for (RobotSide side : RobotSide.values)
-      {
-         RDXSelectablePose3DGizmo footGizmo = new RDXSelectablePose3DGizmo(ReferenceFrame.getWorldFrame(), state.getGoalFootstepToGoalTransform(side));
-         footGizmo.create(baseUI.getPrimary3DPanel());
-         footGizmo.getPoseGizmo().getCenterSphereToTorusRatio().set(0.3f);
-         goalFeetGizmos.put(side, footGizmo);
-
-         RDXFootstepGraphic goalFootGraphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), side);
-         goalFootGraphic.create();
-         goalFeetGraphics.put(side, goalFootGraphic);
-      }
+         goalFeet.put(side, new RDXFootstepPlanActionGoalFootstep(baseUI, side, definition, state, robotModel));
 
       previewFootstepPlan = new RDXFootstepPlanGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints());
 
@@ -240,14 +229,13 @@ public void update()
          goalFocalPointGizmo.getPoseGizmo().setParentFrame(parentFrame);
 
          for (RobotSide side : RobotSide.values)
-            goalFeetGizmos.get(side).getPoseGizmo().setParentFrame(state.getGoalFrame().getReferenceFrame());
+            goalFeet.get(side).setParentFrame();
 
          RDXCRDTTools.syncGizmoWithBidirectionalField(goalStancePointGizmo.getPoseGizmo(), definition.getGoalStancePoint(), definition);
          RDXCRDTTools.syncGizmoWithBidirectionalField(goalFocalPointGizmo.getPoseGizmo(), definition.getGoalFocalPoint(), definition);
 
          for (RobotSide side : RobotSide.values)
-            if (goalFeetGizmos.get(side).getPoseGizmo().getGizmoModifiedByUser().poll())
-               definition.modify();
+            goalFeet.get(side).handleGizmoModifiedByUser();
 
          if (definition.isModified())
             for (RobotSide side : RobotSide.values)
@@ -272,10 +260,7 @@ public void update()
          });
 
          for (RobotSide side : RobotSide.values)
-         {
-            goalFeetGizmos.get(side).getPoseGizmo().update();
-            goalFeetGraphics.get(side).setPose(goalFeetGizmos.get(side).getPoseGizmo().getPose());
-         }
+            goalFeet.get(side).updatePoses();
 
          if (state.getIsNextForExecution())
          {
@@ -344,9 +329,7 @@ public void calculate3DViewPick(ImGui3DViewInput input)
             goalStancePointGizmo.calculate3DViewPick(input);
             goalFocalPointGizmo.calculate3DViewPick(input);
             for (RobotSide side : RobotSide.values)
-            {
-               goalFeetGizmos.get(side).calculate3DViewPick(input);
-            }
+               goalFeet.get(side).calculate3DViewPick(input);
          }
       }
    }
@@ -369,9 +352,7 @@ public void process3DViewInput(ImGui3DViewInput input)
             goalFocalPointGizmo.process3DViewInput(input, false);
             tooltip.setInput(input);
             for (RobotSide side : RobotSide.values)
-            {
-               goalFeetGizmos.get(side).process3DViewInput(input, false);
-            }
+               goalFeet.get(side).process3DViewInput(input);
          }
       }
    }
@@ -433,7 +414,7 @@ protected void renderImGuiWidgetsInternal()
             ImGui.checkbox(labels.get("Focal Point"), goalFocalPointGizmo.getSelected());
             for (RobotSide side : RobotSide.values)
             {
-               ImGui.checkbox(labels.get(side.getPascalCaseName() + " Foot to Goal"), goalFeetGizmos.get(side).getSelected());
+               ImGui.checkbox(labels.get(side.getPascalCaseName() + " Foot to Goal"), goalFeet.get(side).getGizmo().getSelected());
                if (side == RobotSide.LEFT)
                   ImGui.sameLine();
             }
@@ -466,7 +447,7 @@ public void deselectGizmos()
       goalStancePointGizmo.setSelected(false);
       goalFocalPointGizmo.setSelected(false);
       for (RobotSide side : RobotSide.values)
-         goalFeetGizmos.get(side).setSelected(false);
+         goalFeet.get(side).getGizmo().setSelected(false);
       editManuallyPlacedSteps.set(false);
    }
 
@@ -486,7 +467,7 @@ public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
    {
       if (state.areFramesInWorld())
       {
-         if (definition.getIsManuallyPlaced() && (state.getIsNextForExecution() || getSelected()))
+         if (definition.getIsManuallyPlaced())
          {
             for (RDXFootstepPlanActionFootstep footstep : manuallyPlacedFootsteps)
             {
@@ -500,19 +481,10 @@ public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
             goalStancePointGizmo.getVirtualRenderables(renderables, pool);
             goalFocalPointGizmo.getVirtualRenderables(renderables, pool);
             for (RobotSide side : RobotSide.values)
-            {
-               goalFeetGizmos.get(side).getVirtualRenderables(renderables, pool);
-            }
-            for (RobotSide side : RobotSide.values)
-            {
-               goalFeetGraphics.get(side).setHighlighted(footstepsWidget.getIsHovered().get(side));
-               goalFeetGraphics.get(side).getRenderables(renderables, pool);
-            }
+               goalFeet.get(side).getRenderables(renderables, pool, footstepsWidget.getIsHovered().get(side));
 
             if (state.getIsNextForExecution())
-            {
                previewFootstepPlan.getRenderables(renderables, pool);
-            }
          }
       }
    }

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

@@ -0,0 +1,80 @@
+package us.ihmc.rdx.ui.behavior.actions;
+
+import com.badlogic.gdx.graphics.g3d.Renderable;
+import com.badlogic.gdx.utils.Array;
+import com.badlogic.gdx.utils.Pool;
+import us.ihmc.avatar.drcRobot.DRCRobotModel;
+import us.ihmc.behaviors.sequence.actions.FootstepPlanActionDefinition;
+import us.ihmc.behaviors.sequence.actions.FootstepPlanActionState;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.rdx.input.ImGui3DViewInput;
+import us.ihmc.rdx.ui.RDXBaseUI;
+import us.ihmc.rdx.ui.gizmo.RDXSelectablePose3DGizmo;
+import us.ihmc.rdx.ui.graphics.RDXFootstepGraphic;
+import us.ihmc.robotics.robotSide.RobotSide;
+
+/** UI for the behavior footstep plan action's modifiable goal footstep, used by the footstep planner. */
+public class RDXFootstepPlanActionGoalFootstep
+{
+   private final FootstepPlanActionDefinition definition;
+   private final FootstepPlanActionState state;
+
+   private final RDXSelectablePose3DGizmo gizmo;
+   private final RDXFootstepGraphic graphic;
+
+   public RDXFootstepPlanActionGoalFootstep(RDXBaseUI baseUI,
+                                            RobotSide side,
+                                            FootstepPlanActionDefinition definition,
+                                            FootstepPlanActionState state,
+                                            DRCRobotModel robotModel)
+   {
+      this.definition = definition;
+      this.state = state;
+
+      gizmo = new RDXSelectablePose3DGizmo(ReferenceFrame.getWorldFrame(), state.getGoalFootstepToGoalTransform(side));
+      gizmo.create(baseUI.getPrimary3DPanel());
+      gizmo.getPoseGizmo().getCenterSphereToTorusRatio().set(0.3f);
+
+      graphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), side);
+      graphic.create();
+   }
+
+   public void setParentFrame()
+   {
+      gizmo.getPoseGizmo().setParentFrame(state.getGoalFrame().getReferenceFrame());
+   }
+
+   public void handleGizmoModifiedByUser()
+   {
+      if (gizmo.getPoseGizmo().getGizmoModifiedByUser().poll())
+         definition.modify();
+   }
+
+   public void updatePoses()
+   {
+      gizmo.getPoseGizmo().update();
+      graphic.setPose(gizmo.getPoseGizmo().getPose());
+   }
+
+   public void calculate3DViewPick(ImGui3DViewInput input)
+   {
+      gizmo.calculate3DViewPick(input);
+   }
+
+   public void process3DViewInput(ImGui3DViewInput input)
+   {
+      gizmo.process3DViewInput(input, false);
+   }
+
+   public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool, boolean isHighlighted)
+   {
+      gizmo.getVirtualRenderables(renderables, pool);
+      graphic.setHighlighted(isHighlighted);
+      graphic.getRenderables(renderables, pool);
+   }
+
+   public RDXSelectablePose3DGizmo getGizmo()
+   {
+      return gizmo;
+   }
+}

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXFootstepGraphic.java

@@ -29,14 +29,15 @@
 import us.ihmc.scs2.definition.visual.ColorDefinition;
 
 import java.util.ArrayList;
-import java.util.concurrent.atomic.AtomicInteger;
 
+/**
+ * A multiline, colored outline graphic of the foot, typically the 100% support polygon.
+ */
 public class RDXFootstepGraphic implements RenderableProvider
 {
    public static final Color LEFT_FOOT_RED_COLOR = new Color(0.5f, 0.0f, 0.0f, 1.0f);
    public static final Color RIGHT_FOOT_GREEN_COLOR = new Color(0.0f, 0.5f, 0.0f, 1.0f);
    public static final SideDependentList<Color> FOOT_COLORS = new SideDependentList<>(LEFT_FOOT_RED_COLOR, RIGHT_FOOT_GREEN_COLOR);
-   private static final AtomicInteger INDEX = new AtomicInteger();
 
    private final Color color;
    private final Color highlightColor;

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXFootstepPlanGraphic.java

@@ -25,7 +25,6 @@
 import us.ihmc.footstepPlanning.PlannedFootstep;
 import us.ihmc.log.LogTools;
 import us.ihmc.rdx.RDX3DSituatedText;
-import us.ihmc.rdx.mesh.RDXIDMappedColorFunction;
 import us.ihmc.rdx.mesh.RDXMultiColorMeshBuilder;
 import us.ihmc.rdx.tools.LibGDXTools;
 import us.ihmc.rdx.tools.RDXModelBuilder;
@@ -38,24 +37,25 @@
 import java.util.EnumMap;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicReference;
-import java.util.function.Function;
 
+/**
+ * A graphic representing one or more sequential footsteps.
+ * They are colored outlines of the maximum base of support, with a solid fill, and text number on each step.
+ * Optionally, more text can be added to give a name to the footstep plan.
+ *
+ * TODO: Make this class hold a list of RDXFootstepGraphic instead, moving the extra elements here to that class.
+ */
 public class RDXFootstepPlanGraphic implements RenderableProvider
 {
    private final ModelBuilder modelBuilder = new ModelBuilder();
-   RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder();
-   // visualization options
-   private final Function<Integer, Color> colorFunction = new RDXIDMappedColorFunction();
+   private final RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder();
    private final SideDependentList<Color> footstepColors = new SideDependentList<>();
-
    {
       footstepColors.set(RobotSide.LEFT, new Color(RDXFootstepGraphic.LEFT_FOOT_RED_COLOR));
       footstepColors.set(RobotSide.RIGHT, new Color(RDXFootstepGraphic.RIGHT_FOOT_GREEN_COLOR));
    }
-
    private final SideDependentList<ConvexPolygon2D> defaultContactPoints = new SideDependentList<>();
    private volatile Runnable buildMeshAndCreateModelInstance = null;
-
    private ModelInstance modelInstance;
    private Model lastModel;
 

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/FootstepPlanActionExecutor.java

@@ -135,7 +135,7 @@ public void update()
          snappedGoalStancePose.getRotation().set(stanceOrientation);
          snappedGoalStancePose.changeFrame(state.getParentFrame());
 
-         state.getGoalToParentTransform().getValue().set(snappedGoalStancePose);
+         state.getGoalToParentTransform().setValue(snappedGoalStancePose, 1e-5);
          state.getGoalFrame().getReferenceFrame().update();
 
          for (RobotSide side : RobotSide.values)