Merge branch 'develop' into feature/create-height-map-ui

Author: PotatoPeeler3000

.github/workflows/gradle-test-fast.yml

@@ -210,51 +210,3 @@ jobs:
     with:
       subproject: 'simulation-construction-set-tools'
       test-category: 'fast'
-
-  update-evergreen:
-    name: Force update on evergreen
-    runs-on: ubuntu-latest
-    needs: 
-      - alexander
-      - alexander-video
-      - alexander-controller-api
-      - alexander-controller-api-2
-      - alexander-humanoid-behaviors
-      - alexander-humanoid-flat-ground
-      - alexander-humanoid-flat-ground-bullet
-      - alexander-humanoid-obstacle
-      - alexander-humanoid-obstacle-2
-      - alexander-humanoid-push-recovery
-      - alexander-humanoid-rough-terrain
-      - alexander-humanoid-toolbox
-      - example-simulations
-      - ihmc-avatar-interfaces
-      - ihmc-common-walking-control-modules
-      - ihmc-communication
-      - ihmc-footstep-planning
-      - ihmc-graphics
-      - ihmc-high-level-behaviors
-      - ihmc-interfaces
-      - ihmc-java-toolkit
-      - ihmc-manipulation-planning
-      - ihmc-model-file-loader
-      - ihmc-parameter-estimation
-      - ihmc-parameter-optimization
-      - ihmc-path-planning
-      - ihmc-perception
-      - ihmc-robot-data-visualizer
-      - ihmc-robot-models
-      - ihmc-robotics-toolkit
-      - ihmc-ros-tools
-      - ihmc-sensor-processing
-      - ihmc-simulation-toolkit
-      - ihmc-state-estimation
-      - ihmc-system-identification
-      - ihmc-whole-body-controller
-      - robot-environment-awareness
-      - robotiq-hand-drivers
-      - simulation-construction-set-tools
-    if: always() # This ensures the job runs even if one of the dependent jobs fails
-    steps:
-      - name: Call API with curl
-        uses: ihmcrobotics/ihmc-actions/.github/actions/force-update-evergreen@main

.github/workflows/gradle-test-slow.yml

@@ -115,32 +115,3 @@ jobs:
       subproject: 'simulation-construction-set-tools'
       test-category: 'gui-slow'
       requires-self-hosted: true
-
-  update-evergreen:
-    name: Force update on evergreen
-    runs-on: ubuntu-latest
-    needs: 
-      - alexander-allocation-slow
-      - alexander-allocation-slow-2
-      - alexander-controller-api-slow
-      - alexander-controller-api-slow-3
-      - alexander-controller-api-slow-4
-      - alexander-humanoid-behaviors-slow
-      - alexander-humanoid-flat-ground-slow-2
-      - alexander-humanoid-flat-ground-slow-3
-      - alexander-humanoid-flat-ground-slow-4
-      - alexander-humanoid-obstacle-slow
-      - alexander-humanoid-obstacle-slow-2
-      - alexander-humanoid-obstacle-slow-3
-      - alexander-humanoid-push-recovery-slow
-      - alexander-humanoid-rough-terrain-slow
-      - alexander-humanoid-stairs-slow
-      - ihmc-footstep-planning-slow
-      - ihmc-footstep-planning-slow-2
-      - ihmc-java-toolkit-gui-slow
-      - ihmc-path-planning-slow
-      - simulation-construction-set-tools-slow
-    if: always() # This ensures the job runs even if one of the dependent jobs fails
-    steps:
-      - name: Call API with curl
-        uses: ihmcrobotics/ihmc-actions/.github/actions/force-update-evergreen@main

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

@@ -2,5 +2,5 @@
 
 public enum RobotTarget
 {
-   SCS, GAZEBO, REAL_ROBOT, HEAD_ON_A_STICK
+   SCS, GAZEBO, REAL_ROBOT, HEAD_ON_A_STICK, DISPLAY
 }

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

@@ -11,6 +11,7 @@
 import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataReadOnly;
 import us.ihmc.commons.MathTools;
 import us.ihmc.commons.lists.RecyclingArrayList;
+import us.ihmc.commons.thread.Notification;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.packets.MessageTools;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
@@ -26,6 +27,7 @@
 import us.ihmc.humanoidRobotics.communication.packets.KinematicsToolboxMessageFactory;
 import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
 import us.ihmc.mecano.spatial.interfaces.SpatialVectorReadOnly;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
@@ -319,6 +321,7 @@ public void onEntry()
       ikSolverJointGains.setMaximumFeedbackAndMaximumFeedbackRate(angularRateLimit.getValue(), Double.POSITIVE_INFINITY);
 
       tools.resetUserInvalidInputFlag();
+      tools.resetFootContactNotification();
       KinematicsStreamingToolboxParameters kstParameters = tools.getParameters();
       kstParameters.getDefaultSolverConfiguration().setJointVelocityWeight(defaultJointVelocityWeight.getValue());
       kstParameters.getDefaultSolverConfiguration().setJointAccelerationWeight(defaultJointAccelerationWeight.getValue());
@@ -503,6 +506,13 @@ else if (latestInput != null)
             WeightMatrix6D weightMatrix = rigidBodyInput.getWeightMatrix();
             setDefaultWeightsIfNeeded(rigidBodyInput, selectionMatrix, weightMatrix);
 
+            // Skip the blending for the foot, if the user has just started to control the foot
+            RobotSide footSide = getFootSide(rigidBodyInput.getEndEffector(), tools.getDesiredFullRobotModel());
+            if (footSide != null && tools.getStartFootControlNotification(footSide).poll())
+            {
+               rigidBodyControlStartTimeMap.get(rigidBodyInput.getEndEffector()).set(timeInState - streamingBlendingDuration.getValue());
+            }
+
             // Update time for which each rigid body started being controlled.
             YoDouble startTime = rigidBodyControlStartTimeMap.get(rigidBodyInput.getEndEffector());
             if (startTime.isNaN())
@@ -820,6 +830,16 @@ private static void blendWeightMatrix(WeightMatrix3D weightMatrix, double curren
       }
    }
 
+   private static RobotSide getFootSide(RigidBodyReadOnly rigidBody, FullHumanoidRobotModel fullHumanoidRobotModel)
+   {
+      for (RobotSide robotSide : RobotSide.values)
+      {
+         if (rigidBody.equals(fullHumanoidRobotModel.getFoot(robotSide)))
+            return robotSide;
+      }
+      return null;
+   }
+
    @Override
    public void onExit(double timeInState)
    {

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

@@ -17,12 +17,11 @@
 import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataBasics;
 import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataReadOnly;
 import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters;
-import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.ContactableBodiesFactory;
 import us.ihmc.commons.Conversions;
+import us.ihmc.commons.thread.Notification;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
 import us.ihmc.euclid.geometry.interfaces.Pose3DBasics;
-import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
@@ -32,7 +31,6 @@
 import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
 import us.ihmc.euclid.tools.QuaternionTools;
 import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
-import us.ihmc.euclid.tuple3D.Point3D;
 import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
 import us.ihmc.humanoidRobotics.communication.kinematicsStreamingToolboxAPI.KinematicsStreamingToolboxConfigurationCommand;
 import us.ihmc.humanoidRobotics.communication.kinematicsStreamingToolboxAPI.KinematicsStreamingToolboxContactConfigurationCommand;
@@ -42,8 +40,6 @@
 import us.ihmc.humanoidRobotics.communication.kinematicsToolboxAPI.KinematicsToolboxRigidBodyCommand;
 import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
 import us.ihmc.humanoidRobotics.communication.packets.KinematicsToolboxOutputConverter;
-import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
-import us.ihmc.idl.IDLSequence;
 import us.ihmc.log.LogTools;
 import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
@@ -55,13 +51,11 @@
 import us.ihmc.robotModels.FullHumanoidRobotModel;
 import us.ihmc.robotModels.FullHumanoidRobotModelFactory;
 import us.ihmc.robotModels.FullRobotModelUtils;
-import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.robotics.sensors.ForceSensorDefinition;
 import us.ihmc.robotics.sensors.IMUDefinition;
 import us.ihmc.sensorProcessing.communication.packets.dataobjects.RobotConfigurationDataFactory;
-import us.ihmc.wholeBodyController.RobotContactPointParameters;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
@@ -128,6 +122,7 @@ public class KSTTools
    private final KSTInputFilter inputFilter;
 
    private final SideDependentList<Boolean> isFootInSupport = new SideDependentList<>();
+   private final SideDependentList<Notification> startFootControl = new SideDependentList<>(new Notification(), new Notification());
 
    private WholeBodyTrajectoryMessagePublisher trajectoryMessagePublisher = m ->
    {
@@ -289,6 +284,7 @@ public void update()
          }
          ikController.setInitialRobotConfigurationNamedMap(initialConfigurationMap);
          ikController.initialize();
+         resetFootContactNotification();
       }
 
       if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxContactConfigurationCommand.class))
@@ -304,6 +300,8 @@ public void update()
             }
             isFootInSupport.put(side, isFootInContact);
             ikController.setIsFootInSupport(side, isFootInSupport.get(side));
+            if (!isFootInSupport.get(side))
+               startFootControl.get(side).set();
          }
          SteppingParameters steppingParameters = robotModel.getWalkingControllerParameters().getSteppingParameters();
          ikController.updateSupportPolygon(isFootInSupport, steppingParameters.getFootLength(), steppingParameters.getFootWidth(), true);
@@ -383,6 +381,14 @@ public void resetUserInvalidInputFlag()
       invalidUserInput.set(false);
    }
 
+   public void resetFootContactNotification()
+   {
+      for (RobotSide side : RobotSide.values)
+      {
+         startFootControl.get(side).clear();
+      }
+   }
+
    public boolean hasUserSubmittedInvalidInput()
    {
       return invalidUserInput.getValue();
@@ -687,6 +693,11 @@ public boolean isFootInSupport(RobotSide side)
       return isFootInSupport.get(side);
    }
 
+   public Notification getStartFootControlNotification(RobotSide side)
+   {
+      return startFootControl.get(side);
+   }
+
    /**
     * [Unsafe] Copies the joint positions from the given array of joints to the given list of messages.
     * <p>

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

@@ -296,19 +296,14 @@ public static KinematicsStreamingToolboxParameters defaultParameters()
    }
 
    public void setDefault()
-   {
-      setDefault(false);
-   }
-
-   public void setDefault(boolean usingRealtimePlugin)
    {
       clockType = ClockType.CPU_CLOCK;
       toolboxUpdatePeriod = 0.003;
       timeThresholdForSleeping = 3.0;
 
       useStreamingPublisher = true;
       publishingPeriod = 0.006;
-      streamIntegrationDuration = usingRealtimePlugin ? 2.0 * publishingPeriod : 0.1;
+      streamIntegrationDuration = 2.0 * publishingPeriod;
 
       holdChestAngularWeight.set(1.0, 1.0, 0.5);
       holdPelvisLinearWeight.set(10.0, 10.0, 20.0);

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

@@ -16,6 +16,7 @@
 import us.ihmc.commonWalkingControlModules.staticEquilibrium.StabilityMarginRegionCalculator;
 import us.ihmc.commonWalkingControlModules.staticEquilibrium.WholeBodyContactState;
 import us.ihmc.commons.lists.RecyclingArrayList;
+import us.ihmc.commons.thread.Notification;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
 import us.ihmc.concurrent.ConcurrentCopier;

ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java

@@ -181,7 +181,7 @@ public void createToolboxController(DRCRobotModel robotModel)
       new ControllerNetworkSubscriber(toolboxInputTopic, commandInputManager, toolboxOutputTopic, statusOutputManager, toolboxROS2Node);
 
       KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters();
-      parameters.setDefault(false);
+      parameters.setDefault();
       parameters.setToolboxUpdatePeriod(toolboxControllerPeriod);
 
       toolboxController = new KinematicsStreamingToolboxController(commandInputManager,

ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java

@@ -171,6 +171,6 @@ public static ROS2QosProfile getQoS(Class<?> messageClass)
       else if (outputMessageClasses.contains(messageClass))
          return Objects.requireNonNullElse(outputMessageClassSpecificQoS.get(messageClass), ROS2QosProfile.RELIABLE());
       else
-         return ROS2QosProfile.DEFAULT();
+         return ROS2QosProfile.BEST_EFFORT();
    }
 }

ihmc-communication/src/main/java/us/ihmc/communication/ros2log/ROS2LogRecord.java

@@ -50,8 +50,6 @@ else if (requestedState == ROS2LoggerRequestedState.FINISH)
          ROS2Topic<?> ros2Topic = topicsToLog.get(i);
          topicManagers.add(new RecordTopicManager<>(ros2Topic, ros2Node, timestampProvider));
       }
-
-      ThreadTools.sleepForever();
    }
 
    public void start()
@@ -101,6 +99,15 @@ else if (System.currentTimeMillis() - lastReceivedTimestamp.get() > TIMEOUT_MILL
       }
    }
 
+   public void destroy()
+   {
+      if (executorService != null)
+      {
+         executorService.shutdownNow();
+      }
+      ros2Node.destroy();
+   }
+
    public static ROS2Topic<ROS2LogMessage> getROS2LogTopic()
    {
       return ROS2Tools.IHMC_ROOT.withModule(MODULE_NAME).withTypeName(ROS2LogMessage.class);