first commit

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

@@ -5,6 +5,7 @@
 import controller_msgs.msg.dds.WholeBodyStreamingMessage;
 import controller_msgs.msg.dds.WholeBodyTrajectoryMessage;
 import toolbox_msgs.msg.dds.KinematicsToolboxOneDoFJointMessage;
+import toolbox_msgs.msg.dds.UserFootContactStatusMessage;
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.HumanoidKinematicsToolboxController;
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxCommandConverter;
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxController.IKRobotStateUpdater;
@@ -18,6 +19,7 @@
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
+import us.ihmc.concurrent.ConcurrentCopier;
 import us.ihmc.euclid.geometry.interfaces.Pose3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics;
@@ -50,6 +52,7 @@
 import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.robotics.sensors.ForceSensorDefinition;
 import us.ihmc.robotics.sensors.IMUDefinition;
+import us.ihmc.ros2.ROS2Input;
 import us.ihmc.sensorProcessing.communication.packets.dataobjects.RobotConfigurationDataFactory;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
@@ -93,6 +96,9 @@ public class KSTTools
 
    private final YoLong currentMessageId;
 
+   private final ConcurrentCopier<UserFootContactStatusMessage> userFootContactCopier = new ConcurrentCopier<>(UserFootContactStatusMessage::new);
+   private final ROS2Input<UserFootContactStatusMessage> userFootContactStatusSubscription;
+
    private final YoBoolean hasNewInputCommand, hasPreviousInput;
    private final YoDouble latestInputReceivedTime, previousInputReceivedTime;
    private KinematicsStreamingToolboxInputCommand latestInput = null;
@@ -253,6 +259,22 @@ public void update()
          areArmJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isArmJointspaceEnabled(robotSide));
       }
 
+      if (userFootContactStatusSubscription.getMessageNotification().poll())
+      {
+         // Copy the message to a thread-safe location.
+         userFootContactCopier.getCopyForWriting().set(userFootContactStatusSubscription.getLatest());
+         userFootContactCopier.commit();
+      }
+      // Use the thread-safe copy in the main update loop.
+      UserFootContactStatusMessage message = userFootContactCopier.getCopyForReading();
+      if (message != null)
+      {
+         ikController.setUserFootContactState(message.getLeftFootInContact(), message.getRightFootInContact());
+      }
+
+
+
+
       if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxInitialConfigurationCommand.class))
       {
          initCommand.set(commandInputManager.pollNewestCommand(KinematicsStreamingToolboxInitialConfigurationCommand.class));
@@ -266,6 +288,7 @@ public void update()
             initialConfigurationMap.put(jointName, q);
          }
          ikController.setInitialRobotConfigurationNamedMap(initialConfigurationMap);
+         ikController.initialize();
       }
 
       boolean wasRobotUpdated = robotStateUpdater.updateRobotConfiguration(currentRootJoint, currentOneDoFJoint);

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

@@ -5,13 +5,9 @@
 import controller_msgs.msg.dds.RobotConfigurationData;
 import controller_msgs.msg.dds.WholeBodyStreamingMessage;
 import controller_msgs.msg.dds.WholeBodyTrajectoryMessage;
-import toolbox_msgs.msg.dds.KinematicsStreamingToolboxConfigurationMessage;
-import toolbox_msgs.msg.dds.KinematicsStreamingToolboxInitialConfigurationMessage;
-import toolbox_msgs.msg.dds.KinematicsStreamingToolboxInputMessage;
-import toolbox_msgs.msg.dds.KinematicsToolboxConfigurationMessage;
-import toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus;
-import toolbox_msgs.msg.dds.ToolboxStateMessage;
+import toolbox_msgs.msg.dds.*;
 import us.ihmc.avatar.drcRobot.DRCRobotModel;
+import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.HumanoidKinematicsToolboxController;
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxController.RobotConfigurationDataBasedUpdater;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxController;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
@@ -129,6 +125,22 @@ public void registerExtraPuSubs(ROS2Node ros2Node)
          s.takeNextData(robotConfigurationData, null);
          robotStateUpdater.setRobotConfigurationData(robotConfigurationData);
       });
+      // NEW: Subscribe to the user foot contact status message.
+      UserFootContactStatusMessage userFootContactStatus = new UserFootContactStatusMessage();
+      ros2Node.createSubscription(getInputTopic().withTypeName(UserFootContactStatusMessage.class), s ->
+      {
+         // Get the internal IK solver via the new getter method.
+         HumanoidKinematicsToolboxController ikController = controller.getTools().getIKController();
+
+         if (ikController != null)
+         {
+            s.takeNextData(userFootContactStatus, null);
+            // Directly call the setter on the IK controller.
+            ikController.setUserFootContactState(userFootContactStatus.getLeftFootInContact(),
+                    userFootContactStatus.getRightFootInContact());
+         }
+      });
+
 
       CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus();
 

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

@@ -1111,23 +1111,35 @@ public void restrictZeroCrossover(DRCRobotModel robotModel, String jointName)
       double positionLowerLimit = joint.getPositionLowerLimit();
       double positionUpperLimit = joint.getPositionUpperLimit();
 
-      /* No zero cross-over */
-      if (positionLowerLimit * positionUpperLimit > 0.0)
-         return;
-
-      double tolerance = 0.1;
-      boolean adjustLowerLimit = Math.abs(positionLowerLimit) < Math.abs(positionUpperLimit);
-      if (adjustLowerLimit)
-      {
-         if (jointCustomPositionLowerLimits == null)
-            jointCustomPositionLowerLimits = new HashMap<>();
-         jointCustomPositionLowerLimits.put(jointName, tolerance);
-      }
-      else
+//      /* No zero cross-over */
+//      if (positionLowerLimit * positionUpperLimit > 0.0)
+//         return;
+
+//      double tolerance = 0.1;
+//      boolean adjustLowerLimit = Math.abs(positionLowerLimit) < Math.abs(positionUpperLimit);
+//      if (adjustLowerLimit)
+//      {
+//         if (jointCustomPositionLowerLimits == null)
+//            jointCustomPositionLowerLimits = new HashMap<>();
+//         jointCustomPositionLowerLimits.put(jointName, tolerance);
+//      }
+//      else
+//      {
+//         if (jointCustomPositionUpperLimits == null)
+//            jointCustomPositionUpperLimits = new HashMap<>();
+//         jointCustomPositionUpperLimits.put(jointName, -tolerance);
+//      }
+      if (jointName.toLowerCase().contains("elbow"))
       {
          if (jointCustomPositionUpperLimits == null)
             jointCustomPositionUpperLimits = new HashMap<>();
-         jointCustomPositionUpperLimits.put(jointName, -tolerance);
+         jointCustomPositionUpperLimits.put(jointName, 1.422);
+      }
+      else if (jointName.toLowerCase().contains("knee"))
+      {
+         if (jointCustomPositionLowerLimits == null)
+            jointCustomPositionLowerLimits = new HashMap<>();
+         jointCustomPositionLowerLimits.put(jointName, 0.152);
       }
    }
 

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java

@@ -138,6 +138,8 @@ public class HumanoidKinematicsToolboxController extends KinematicsToolboxContro
     * {@link HumanoidKinematicsToolboxConfigurationMessage}.
     */
    private final YoBoolean holdCenterOfMassXYPosition = new YoBoolean("holdCenterOfMassXYPosition", registry);
+
+   private final SideDependentList<YoBoolean> isUserFootInContact = new SideDependentList<>();
    /**
     * Default weight used when holding a support rigid-body in place. It is rather high such that they
     * do not deviate much from their initial position/pose.
@@ -252,6 +254,11 @@ public HumanoidKinematicsToolboxController(CommandInputManager commandInputManag
          setupVisualization(desiredFullRobotModel.getFoot(robotSide));
       }
 
+      for (RobotSide robotSide : RobotSide.values)
+      {
+         isUserFootInContact.put(robotSide, new YoBoolean("isUser" + robotSide.getPascalCaseName() + "FootInContact", registry));
+      }
+
       for (RobotSide robotSide : RobotSide.values)
       {
          String side = robotSide.getCamelCaseNameForMiddleOfExpression();
@@ -342,7 +349,13 @@ public void setInitialRobotConfiguration(DRCRobotModel robotModel)
 
       setInitialRobotConfiguration(privilegedConfiguration);
    }
-
+   public void setUserFootContactState(boolean leftFootInContact, boolean rightFootInContact)
+   {
+      if (isUserFootInContact.get(RobotSide.LEFT) != null)
+         isUserFootInContact.get(RobotSide.LEFT).set(leftFootInContact);
+      if (isUserFootInContact.get(RobotSide.RIGHT) != null)
+         isUserFootInContact.get(RobotSide.RIGHT).set(rightFootInContact);
+   }
    public void setCollisionModel(RobotCollisionModel collisionModel)
    {
       if (collisionModel != null)
@@ -629,7 +642,7 @@ private void addHoldSupportEndEffectorCommands(FeedbackControlCommandBuffer buff
          RigidBodyBasics foot = desiredFullRobotModel.getFoot(robotSide);
 
          // Do not hold the foot position if the user is already controlling it.
-         if (isFootInSupport.get(robotSide).getBooleanValue() && !isUserControllingRigidBody(foot))
+         if (isFootInSupport.get(robotSide).getBooleanValue() && !isUserControllingRigidBody(foot) && isUserFootInContact.get(RobotSide.LEFT).getValue() && isUserFootInContact.get(RobotSide.RIGHT).getValue() )
          {
             SpatialFeedbackControlCommand feedbackControlCommand = bufferToPack.addSpatialFeedbackControlCommand();
             feedbackControlCommand.set(rootBody, foot);

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxController.java

@@ -5,6 +5,7 @@
 import toolbox_msgs.msg.dds.HumanoidKinematicsToolboxConfigurationMessage;
 import toolbox_msgs.msg.dds.KinematicsToolboxConfigurationMessage;
 import toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus;
+import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.KSTTools;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxController;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxModule;
 import us.ihmc.commonWalkingControlModules.configurations.JointPrivilegedConfigurationParameters;
@@ -571,6 +572,12 @@ public void setInitialRobotConfiguration(Map<OneDoFJointBasics, Double> initialR
       this.initialRobotConfigurationMap = new TObjectDoubleHashMap<>();
       initialRobotConfigurationMap.forEach((key, value) -> this.initialRobotConfigurationMap.put(key, value));
    }
+   public HumanoidKinematicsToolboxController getHumanoidIKController()
+   {
+      // Assuming the internal KSTools object is named 'tools'
+      return KSTTools.();
+   }
+
 
    /**
     * Sets up the robot configuration this IK should start from when initializing.

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/ui/RDXBaseUI.java

@@ -255,7 +255,7 @@ public void launchRDXApplication(Lwjgl3ApplicationAdapter applicationAdapter)
 
    public void create()
    {
-      create(RDXSceneLevel.MODEL, RDXSceneLevel.VIRTUAL);
+      create(RDXSceneLevel.MODEL, RDXSceneLevel.VIRTUAL, RDXSceneLevel.GROUND_TRUTH);
    }
 
    public void create(RDXSceneLevel... sceneLevels)

ihmc-graphics/src/libgdx/resources/vr/actions.json

@@ -134,7 +134,7 @@
    ],
    "default_bindings" : [
       {
-         "binding_url" : "index_bindings.json",
+         "binding_url" : "vive_focus3_bindings.json",
          "controller_type" : "knuckles"
       }
    ],

ihmc-graphics/src/libgdx/resources/vr/tracker_roles.json

@@ -1,8 +1,8 @@
 {
-  "Chest" : "",
-  "Left Ankle" : "LHR-41A915A6",
+  "Chest" : "3B-3AK00484",
+  "Left Ankle" : "3B-3AK00133",
   "Left Wrist" : "",
-  "Waist" : "",
-  "Right Ankle" : "LHR-743512BE",
+  "Waist" : "3B-3AK00134",
+  "Right Ankle" : "3B-3AK00141",
   "Right Wrist" : ""
 }

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

@@ -451,9 +451,6 @@ private void calculateVRPick(RDXVRContext vrContext)
 
          if (interactablesAvailable && showContactCollisionMeshes.get())
             contactCollisionModel.calculateVRPick(vrContext);
-
-         for (RDXInteractableRobotLink robotPartInteractable : allInteractableRobotLinks)
-            robotPartInteractable.calculateVRPick(vrContext);
       }
    }