Feature/alex teleop 2

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KSTTools.java

@@ -267,7 +267,7 @@ public void update()
       {
          areHandTaskspaceOutputsEnabled.get(robotSide).set(configurationCommand.isHandTaskspaceEnabled(robotSide));
          areArmJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isArmJointspaceEnabled(robotSide));
-         areLegJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isLegJointspaceEnabled(robotSide));
+         areLegJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isLegJointspaceEnabled(robotSide) && !isFootInSupport.get(robotSide));
       }
 
       if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxInitialConfigurationCommand.class))

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

@@ -91,9 +91,6 @@ private void updateGhostPoseWithJoystick()
    private void updateGhostPitchBasedOnFeet()
    {
       miniGhostFullRobotModel.getElevator().updateFramesRecursively();
-      miniGhostFullRobotModel.getRootJoint().getJointPose().getRotation();
-      miniGhostFullRobotModel.getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame().getRotation();
-      miniGhostFullRobotModel.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame().getRotation();
 
       Quaternion rootJointInitialOrientation = new Quaternion(miniGhostFrame.getTransformToRoot().getRotation());
       // Get the rotations of the sole frames.

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

@@ -16,6 +16,7 @@
 import us.ihmc.euclid.referenceFrame.FramePose3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.tuple3D.Point3D;
+import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.motionRetargeting.RetargetingParameters;
@@ -43,7 +44,19 @@
 import java.util.Collection;
 import java.util.List;
 import java.util.Set;
-
+/**
+ * Manages the various VR modes and associated state for immersive humanoid control in the RDX UI.
+ * <p>
+ * Responsible for switching between modes such as whole-body IK streaming, footstep placement,
+ * joystick-based walking, and footstep streaming; maintaining their corresponding input logic,
+ * GUIs, and visualizations in the VR context.
+ * </p>
+ * <p>
+ * Handles creation and update of VR panels, occlusion and perception visualizers, hand managers, and robot
+ * streaming helpers; integrates robot-side per-arm and hand tracking, stereo panel occlusion for rendering,
+ * and dynamic control panel presentation.
+ * </p>
+ */
 public class RDXVRModeManager
 {
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
@@ -71,6 +84,7 @@ public class RDXVRModeManager
    private final Throttler panelOcclusionRateLimiter = new Throttler();
    private final SideDependentList<List<RigidBodyBasics>> syncedRobotArmRigidBodies = new SideDependentList<>(new ArrayList<>(), new ArrayList<>());
    private final SideDependentList<List<RigidBodyBasics>> ghostIKRobotArmRigidBodies = new SideDependentList<>(new ArrayList<>(), new ArrayList<>());
+   private final SideDependentList<Point3D> tempHandPoints = new SideDependentList<>(new Point3D(), new Point3D());
 
    public void create(RDXBaseUI baseUI,
                       ROS2SyncedRobotModel syncedRobot,
@@ -219,8 +233,6 @@ private void processVRInput(RDXVRContext vrContext)
    public void update()
    {
       vrManager.getTeleporter().setBButtonEnabled(mode != RDXVRMode.WHOLE_BODY_IK_STREAMING);
-      if (mode != RDXVRMode.INPUTS_DISABLED)
-         interactablesEnabled.set(false);
 
       switch (mode)
       {
@@ -250,15 +262,54 @@ public void update()
             for (RobotSide side : RobotSide.values)
             {
                checkStereoPanelOcclusions(syncedRobotArmRigidBodies.get(side), perceptionVisualizers.getRobotVisualizer().getMultiBodyGraphic());
+               Point3D tempPoint = tempHandPoints.get(side);
+               tempPoint.set(syncedRobot.getFullRobotModel().getHandControlFrame(side).getTransformToRoot().getTranslation());
+               checkStereoPanelOcclusionWithHandControlFrames(tempPoint, syncedRobotArmRigidBodies.get(side), perceptionVisualizers.getRobotVisualizer().getMultiBodyGraphic());
                if (mode == RDXVRMode.WHOLE_BODY_IK_STREAMING && kinematicsStreaming != null)
                {
                   checkStereoPanelOcclusions(ghostIKRobotArmRigidBodies.get(side), kinematicsStreaming.getGhostRobotGraphic());
+                  tempPoint.set(kinematicsStreaming.getGhostFullRobotModel().getHandControlFrame(side).getTransformToRoot().getTranslation());
+                  checkStereoPanelOcclusionWithHandControlFrames(tempPoint, ghostIKRobotArmRigidBodies.get(side), kinematicsStreaming.getGhostRobotGraphic());
                }
             }
          }
       }
    }
 
+   /**
+    * Checks if the specified hand control frame point is occluded by the stereo panel's view.
+    * <p>
+    * If it is occluded, hides the last rigid body in the provided list, which is closest to the hand control frame.
+    * If it is not occluded, ensures that rigid body is visible.
+    * </p>
+    *
+    * @param handPoint     the hand control frame point to check for occlusion
+    * @param rigidBodyList the ordered list of rigid bodies for the arm; last entry is closest to hand
+    * @param robotGraphics the graphics object whose visibility set should be updated
+    */
+   private void checkStereoPanelOcclusionWithHandControlFrames(Point3DReadOnly handPoint, List<RigidBodyBasics> rigidBodyList, RDXMultiBodyGraphic robotGraphics)
+   {
+      if (stereoPanel.isOccludingView(handPoint))
+      {
+         robotGraphics.getMultiBody().getRigidBodiesToHide().add(rigidBodyList.get(rigidBodyList.size() - 1).getName());
+      }
+      else
+      {
+         robotGraphics.getMultiBody().getRigidBodiesToHide().remove(rigidBodyList.get(rigidBodyList.size() - 1).getName());
+      }
+   }
+
+   /**
+    * Checks occlusion of the origins of a list of robot rigid bodies against the stereo panel's view.
+    * <p>
+    * For each {@link RigidBodyBasics} in the list, this method tests if its origin (expressed as a {@link Point3D})
+    * is currently occluded by the stereo panel. If occluded, the rigid body's name is added to the
+    * set of hidden bodies in the provided {@code robotGraphics}. If not occluded, its name is removed from the set.
+    * </p>
+    *
+    * @param rigidBodyList the list of rigid bodies to test for occlusion by the stereo panel
+    * @param robotGraphics the graphics instance whose hidden body set is updated for visualization
+    */
    private void checkStereoPanelOcclusions(List<RigidBodyBasics> rigidBodyList, RDXMultiBodyGraphic robotGraphics)
    {
       for (RigidBodyBasics rigidBody : rigidBodyList)
@@ -280,23 +331,22 @@ public void renderImGuiWidgets()
       if (ImGui.radioButton(labels.get(RDXVRMode.INPUTS_DISABLED.getReadableName()), mode == RDXVRMode.INPUTS_DISABLED))
       {
          mode = RDXVRMode.INPUTS_DISABLED;
-         if (kinematicsStreaming != null)
-            kinematicsStreaming.setKSTEnabled(false);
+         disableKinematicsStreaming();
          footstepPlacer.reset();
          footstepStreaming.reset();
       }
       if (ImGui.radioButton(labels.get(RDXVRMode.FOOTSTEP_PLACEMENT.getReadableName()), mode == RDXVRMode.FOOTSTEP_PLACEMENT))
       {
          mode = RDXVRMode.FOOTSTEP_PLACEMENT;
-         if (kinematicsStreaming != null)
-            kinematicsStreaming.setKSTEnabled(false);
+         disableKinematicsStreaming();
+         interactablesEnabled.set(false);
       }
       if (ImGui.radioButton(labels.get(RDXVRMode.FOOTSTEP_STREAMING.getReadableName()), mode == RDXVRMode.FOOTSTEP_STREAMING))
       {
          mode = RDXVRMode.FOOTSTEP_STREAMING;
-         if (kinematicsStreaming != null)
-            kinematicsStreaming.setKSTEnabled(false);
+         disableKinematicsStreaming();
          footstepStreaming.reset();
+         interactablesEnabled.set(false);
       }
       if (kinematicsStreaming == null)
       {
@@ -307,6 +357,7 @@ public void renderImGuiWidgets()
          mode = RDXVRMode.WHOLE_BODY_IK_STREAMING;
          footstepPlacer.reset();
          footstepStreaming.reset();
+         interactablesEnabled.set(false);
       }
       if (kinematicsStreaming == null)
       {
@@ -315,12 +366,18 @@ public void renderImGuiWidgets()
       if (ImGui.radioButton(labels.get(RDXVRMode.JOYSTICK_WALKING.getReadableName()), mode == RDXVRMode.JOYSTICK_WALKING))
       {
          mode = RDXVRMode.JOYSTICK_WALKING;
-         if (kinematicsStreaming != null)
-            kinematicsStreaming.setKSTEnabled(false);
+         disableKinematicsStreaming();
          footstepPlacer.reset();
+         interactablesEnabled.set(false);
       }
    }
 
+   private void disableKinematicsStreaming()
+   {
+      if (kinematicsStreaming != null)
+         kinematicsStreaming.setKSTEnabled(false);
+   }
+
    private void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool, Set<RDXSceneLevel> sceneLevels)
    {
       if (sceneLevels.contains(RDXSceneLevel.VIRTUAL))

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

@@ -321,6 +321,7 @@ public void processVRInput()
          KinematicsStreamingToolboxInputMessage toolboxInputMessage = new KinematicsStreamingToolboxInputMessage();
          processControllers(toolboxInputMessage);
          processTrackers(toolboxInputMessage);
+         processHeadset(toolboxInputMessage);
          retargetMotion(toolboxInputMessage);
          multiContact.doCoMControl(toolboxInputMessage);
 
@@ -530,6 +531,29 @@ private void processTrackers(KinematicsStreamingToolboxInputMessage messageToPac
       }
    }
 
+   private void processHeadset(KinematicsStreamingToolboxInputMessage messageToPack)
+   {
+      vrContext.getHeadset().runIfConnected(headset->
+      {
+         RigidBodyBasics controlledSegment = getControlledSegment(HEAD);
+         if (controlledSegment != null)
+         {
+            FramePose3D desiredPose = new FramePose3D();
+            desiredPose.setToZero(headset.getXForwardZUpHeadsetFrame());
+            desiredPose.changeFrame(ReferenceFrame.getWorldFrame());
+            KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(controlledSegment,
+                                                                               desiredPose,
+                                                                               headset.getAngularVelocity(),
+                                                                               headset.getLinearVelocity(),
+                                                                               retargetingParameters.getPositionWeight(HEAD),
+                                                                               retargetingParameters.getOrientationWeight(HEAD),
+                                                                               retargetingParameters.getLinearRateLimitation(HEAD),
+                                                                               retargetingParameters.getAngularRateLimitation(HEAD));
+            messageToPack.getInputs().add().set(message);
+         }
+      });
+   }
+
    private void retargetMotion(KinematicsStreamingToolboxInputMessage messageToPack)
    {
       if (armScaling.get())
@@ -608,6 +632,7 @@ private RigidBodyBasics getControlledSegment(VRTrackedSegmentType segmentType)
          case LEFT_HAND, RIGHT_HAND -> ghostFullRobotModel.getHand(segmentType.getSegmentSide());
          case LEFT_ANKLE, RIGHT_ANKLE -> ghostFullRobotModel.getFoot(segmentType.getSegmentSide());
          case LEFT_WRIST, RIGHT_WRIST -> ghostFullRobotModel.getForearm(segmentType.getSegmentSide());
+         case HEAD -> ghostFullRobotModel.getHead();
          case CHEST -> ghostFullRobotModel.getChest();
          case WAIST -> ghostFullRobotModel.getPelvis();
       };

ihmc-high-level-behaviors/src/main/java/us/ihmc/motionRetargeting/VRTrackedSegmentType.java

@@ -8,6 +8,7 @@ public enum VRTrackedSegmentType
    RIGHT_HAND("Right Hand", RobotSide.RIGHT),
    LEFT_WRIST("Left Wrist", RobotSide.LEFT),
    RIGHT_WRIST("Right Wrist", RobotSide.RIGHT),
+   HEAD("Head", null),
    CHEST("Chest", null),
    WAIST("Waist", null),
    LEFT_ANKLE("Left Ankle", RobotSide.LEFT),

open-alexander/src/main/java/us/ihmc/openAlexander/parameters/controller/OpenAlexanderMomentumOptimizationSettings.java

@@ -54,8 +54,7 @@ public class OpenAlexanderMomentumOptimizationSettings extends MomentumOptimizat
    // TODO Needs tune up.
    private final Vector2D copRateHighWeight = new Vector2D(0.00008, 0.00032);
 
-   // TODO Needs tune up.
-   private final double neckJointspaceWeight = 0.0;
+   private final double neckJointspaceWeight = 50.0;
    // TODO Needs tune up.
    private final double spineJointspaceWeight = 10.0;
    // TODO Needs tune up.