From 17012df073b299f1cce10321fd25ca1eb7a9362f Mon Sep 17 00:00:00 2001 From: Stephen McCrory Date: Fri, 28 Feb 2025 10:47:00 -0600 Subject: [PATCH 1/2] Adding KST param to deactivate joints, similar to controller --- .../IKStreamingRTPluginFactory.java | 7 +++++++ .../KinematicsStreamingToolboxController.java | 6 ++++++ .../KinematicsStreamingToolboxModule.java | 5 +++++ .../KinematicsStreamingToolboxParameters.java | 9 +++++++++ 4 files changed, 27 insertions(+) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java index d9aa7627852..c99e42423ca 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java @@ -224,6 +224,13 @@ public IKStreamingRTThread(String robotName, fullRobotModelFactory, yoGraphicsListRegistry, registry); + + List inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + kinematicsStreamingToolboxController.getActiveOptimizationSettings().deactivateJoint(desiredFullRobotModel.getOneDoFJointByName(inactiveJoints.get(i))); + } + kinematicsStreamingToolboxController.setCollisionModel(collisionModel); MessageUnpacker wholeBodyStreamingMessageUnpacker = MessageUnpackingTools.createWholeBodyStreamingMessageUnpacker(); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java index 0f28542f294..74f138be4be 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java @@ -8,6 +8,7 @@ import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxModule; import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.KinematicsStreamingToolboxParameters.ClockType; import us.ihmc.avatar.networkProcessor.modules.ToolboxController; +import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseKinematics.InverseKinematicsOptimizationSettingsCommand; import us.ihmc.commons.Conversions; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; @@ -253,6 +254,11 @@ public void setRobotStateUpdater(IKRobotStateUpdater robotStateUpdater) tools.setRobotStateUpdater(robotStateUpdater); } + public InverseKinematicsOptimizationSettingsCommand getActiveOptimizationSettings() + { + return tools.getIKController().getActiveOptimizationSettings(); + } + public void updateCapturabilityBasedStatus(CapturabilityBasedStatus newStatus) { tools.updateCapturabilityBasedStatus(newStatus); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java index 626707f08e1..8298d4748b8 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java @@ -82,6 +82,11 @@ public KinematicsStreamingToolboxModule(DRCRobotModel robotModel, registry); controller.setRobotStateUpdater(robotStateUpdater); controller.setCollisionModel(robotModel.getHumanoidRobotKinematicsCollisionModel()); + List inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + controller.getActiveOptimizationSettings().deactivateJoint(controller.getDesiredFullRobotModel().getOneDoFJointByName(inactiveJoints.get(i))); + } Map initialConfiguration = fromStandPrep(robotModel); if (initialConfiguration != null) controller.setInitialRobotConfigurationNamedMap(initialConfiguration); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java index 28cbe1d59c1..842c396609e 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java @@ -9,6 +9,8 @@ import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.commons.UnitConversions; +import java.util.ArrayList; +import java.util.List; import java.util.Map; public class KinematicsStreamingToolboxParameters @@ -269,6 +271,8 @@ public enum ClockType private double solverPrivilegedDefaultWeight; private double solverPrivilegedDefaultGain; + private final List inactiveJoints = new ArrayList<>(); + public static KinematicsStreamingToolboxParameters defaultParameters() { KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters(); @@ -1059,4 +1063,9 @@ public void setSolverPrivilegedDefaultGain(double solverPrivilegedDefaultGain) { this.solverPrivilegedDefaultGain = solverPrivilegedDefaultGain; } + + public List getInactiveJoints() + { + return inactiveJoints; + } } From fe13cc255232e4a18a284b8b57ae5d1b3cf14b1c Mon Sep 17 00:00:00 2001 From: Stephen McCrory Date: Tue, 11 Feb 2025 09:51:02 -0600 Subject: [PATCH 2/2] Publishing com region and sensitivity by default and adding corresponding visualizers --- .../HumanoidKinematicsToolboxController.java | 13 +- ...abilityMarginKinematicsCostCalculator.java | 36 ++++-- .../RDXMultiContactRegionGraphic.java | 120 +++++++++++------- .../ui/vr/RDXVRKinematicsStreamingMode.java | 2 +- .../msg/KinematicsToolboxOutputStatus_.idl | 5 + ...PlanningToolboxOutputStatusPubSubType.java | 2 +- .../dds/KinematicsToolboxOutputStatus.java | 27 ++++ ...nematicsToolboxOutputStatusPubSubType.java | 13 +- ...trollerPreviewOutputMessagePubSubType.java | 2 +- ...ToolboxConfigurationMessagePubSubType.java | 2 +- ...odyTrajectoryToolboxMessagePubSubType.java | 2 +- ...ajectoryToolboxOutputStatusPubSubType.java | 2 +- .../msg/KinematicsToolboxOutputStatus.msg | 3 + .../msg/KinematicsToolboxOutputStatus.msg | 4 + 14 files changed, 163 insertions(+), 70 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java index 2e7cb1ae6f2..28aa175a8df 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java @@ -69,7 +69,6 @@ public class HumanoidKinematicsToolboxController extends KinematicsToolboxController { private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); - private static final Vector3D zeroVector = new Vector3D(); private static final double FOOT_COEFFICIENT_OF_FRICTION = 0.8; private static final double HAND_COEFFICIENT_OF_FRICTION = 0.4; @@ -245,6 +244,9 @@ public HumanoidKinematicsToolboxController(CommandInputManager commandInputManag setupVisualization(desiredFullRobotModel.getFoot(robotSide)); } + setupVisualization(desiredFullRobotModel.getPelvis()); + setupVisualization(desiredFullRobotModel.getChest()); + for (RobotSide robotSide : RobotSide.values) { String side = robotSide.getCamelCaseNameForMiddleOfExpression(); @@ -529,14 +531,10 @@ public void updateInternal() if (multiContactRegionCalculator.hasSolvedWholeRegion()) { activeContactPointPositions.clear(); - getSolution().getSupportRegion().clear(); - for (int i = 0; i < multiContactRegionCalculator.getNumberOfVertices(); i++) { activeContactPointPositions.add().set(multiContactRegionCalculator.getOptimizedVertex(i)); - getSolution().getSupportRegion().add().set(multiContactRegionCalculator.getOptimizedVertex(i)); } - updateSupportPolygonConstraint(activeContactPointPositions); } } @@ -733,6 +731,7 @@ private void processCapturabilityBasedStatus(CapturabilityBasedStatus capturabil } // CoM constraint polygon is the convex hull of the feet contact points. Even when upper body is load-bearing, initialize to this. + activeContactPointPositions.clear(); Object leftFootSupportPolygon2d = capturabilityBasedStatus.getLeftFootSupportPolygon3d(); Object rightFootSupportPolygon2d = capturabilityBasedStatus.getRightFootSupportPolygon3d(); for (int i = 0; i < leftFootSupportPolygon2d.size(); i++) @@ -813,7 +812,9 @@ protected void getAdditionalFeedbackControlCommands(FeedbackControlCommandBuffer { addHoldSupportEndEffectorCommands(bufferToPack); addHoldCenterOfMassXYCommand(bufferToPack); - stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack); + + double supportRegionSensitivity = stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack); + getSolution().setSupportRegionSensitivity(supportRegionSensitivity); } @Override diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java index 435627ddcf7..dc3d20e9ead 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java @@ -25,6 +25,7 @@ public class StabilityMarginKinematicsCostCalculator { private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + private static final double NULL_POSTURAL_SENSITIVITY = -1.0; private final WholeBodyContactState wholeBodyContactState; private final StabilityMarginRegionCalculator multiContactRegionCalculator; @@ -65,7 +66,10 @@ public StabilityMarginKinematicsCostCalculator(WholeBodyContactState wholeBodyCo pelvisControlFramePose.setToZero(afterRootJointFrame); pelvisControlFramePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); - this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel, wholeBodyContactState, multiContactRegionCalculator, registry); + this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel, + wholeBodyContactState, + multiContactRegionCalculator, + registry); parentRegistry.addChild(registry); } @@ -74,26 +78,32 @@ public void setEnabled(boolean enable) isEnabled.set(enable); } - public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack) + public boolean isEnabled() { - if (!isEnabled.getBooleanValue() || !isUpperBodyLoadBearing.getValue()) - return; + return isEnabled.getBooleanValue(); + } - double stabilityMargin = multiContactRegionCalculator.getStabilityMargin(); - if (stabilityMargin > maxMarginToPenalize.getValue()) - return; + /** + * Computes and packs the feedback objective. Returns the postural sensitivity + */ + public double addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack) + { + if (!isUpperBodyLoadBearing.getValue() || !multiContactRegionCalculator.hasSolvedWholeRegion()) + return NULL_POSTURAL_SENSITIVITY; + double stabilityMargin = multiContactRegionCalculator.getStabilityMargin(); double deltaStabilityMargin = stabilityMargin - minStabilityMargin.getValue(); double alpha = EuclidCoreTools.clamp(deltaStabilityMargin / minStabilityMargin.getValue(), 0.0, 1.0); double weight = alpha * stabilityMarginWeight.getValue(); stabilityGradientCalculator.update(); - DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient(); - double stabilityGradientMagnitude = CommonOps_DDRM.dot(stabilityMarginGradient, stabilityMarginGradient); - if (stabilityGradientMagnitude < 1.0e-5) - return; + double posturalSensitivity = stabilityGradientCalculator.getPostureSensitivity(); + + if (!isEnabled.getValue() || posturalSensitivity < 1.0e-3) + return posturalSensitivity; - double gradientScalar = desiredStabilityMarginVelocity.getValue() / stabilityGradientMagnitude; + DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient(); + double gradientScalar = desiredStabilityMarginVelocity.getValue() / EuclidCoreTools.square(posturalSensitivity); // Feed-forward joint velocities OneDoFJointBasics[] oneDoFJoints = wholeBodyContactState.getOneDoFJoints(); @@ -131,5 +141,7 @@ public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack spatialFeedbackControlCommand.getGains().getPositionGains().setDerivativeGains(0.0); spatialFeedbackControlCommand.getGains().getOrientationGains().setProportionalGains(0.0); spatialFeedbackControlCommand.getGains().getOrientationGains().setDerivativeGains(0.0); + + return posturalSensitivity; } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java index f82c67ec579..694643cc203 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java @@ -31,21 +31,22 @@ public class RDXMultiContactRegionGraphic implements RenderableProvider { - private static final Color POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933"); + private static final Color NOMINAL_POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933"); + private static final Color LOW_STABILITY_POLYGON_GRAPHIC_COLOR = Color.valueOf("EB3D40"); private static final double STABILITY_GRAPHIC_HEIGHT = 2.0; - private final ConvexPolygon2D multiContactSupportRegion = new ConvexPolygon2D(); + private final ConvexPolygon2D supportRegion = new ConvexPolygon2D(); private final FramePoint3D comCurrent = new FramePoint3D(); + private final FramePoint3D comDesired = new FramePoint3D(); private final FramePoint3D comXYAtFootHeight = new FramePoint3D(); - private final ConvexPolygon2D closestProximityEdge = new ConvexPolygon2D(); + private final FramePoint3D desiredCoMXYAtFootHeight = new FramePoint3D(); private int minimumEdgeIndex; private double minimumEdgeDistance; private final ModelBuilder modelBuilder = new ModelBuilder(); private final RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder(); - private final FullHumanoidRobotModel ghostFullRobotModel; private final CenterOfMassReferenceFrame centerOfMassFrame; private final MidFrameZUpFrame midFeetZUpFrame; private final RigidBodyTransform transform = new RigidBodyTransform(); @@ -55,7 +56,6 @@ public class RDXMultiContactRegionGraphic implements RenderableProvider public RDXMultiContactRegionGraphic(FullHumanoidRobotModel ghostFullRobotModel) { - this.ghostFullRobotModel = ghostFullRobotModel; this.centerOfMassFrame = new CenterOfMassReferenceFrame("ghostCoMFrame", ReferenceFrame.getWorldFrame(), ghostFullRobotModel.getRootBody()); this.midFeetZUpFrame = new MidFrameZUpFrame("midFeedZUpWhost", ReferenceFrame.getWorldFrame(), @@ -63,60 +63,90 @@ public RDXMultiContactRegionGraphic(FullHumanoidRobotModel ghostFullRobotModel) ghostFullRobotModel.getSoleFrame(RobotSide.RIGHT)); } - public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus) + public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus, FramePoint3D desiredCoMPosition) { - multiContactSupportRegion.clear(); - Object supportRegion = kinematicsToolboxOutputStatus.getSupportRegion(); - - for (int i = 0; i < supportRegion.size(); i++) - { - multiContactSupportRegion.addVertex(supportRegion.get(i)); - } - - multiContactSupportRegion.update(); - - if (multiContactSupportRegion.getNumberOfVertices() < 3) - { - modelInstance = null; - lastModel = null; - return; - } - meshBuilder.clear(); + // Update frames and compute mid-feet height centerOfMassFrame.update(); midFeetZUpFrame.update(); - updateMinimumEdge(); FramePoint3D midFoot = new FramePoint3D(midFeetZUpFrame); midFoot.changeFrame(ReferenceFrame.getWorldFrame()); double footZ = midFoot.getZ(); - transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ); - - for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++) + // Update desired and achieved CoM graphic { - Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i); - Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i); + comCurrent.setToZero(centerOfMassFrame); + comCurrent.changeFrame(ReferenceFrame.getWorldFrame()); + + meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK); + + comXYAtFootHeight.setIncludingFrame(comCurrent); + comXYAtFootHeight.setZ(footZ); + meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK); + + FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight); + comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); + meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK); + + if (desiredCoMPosition != null) + { + comDesired.setMatchingFrame(desiredCoMPosition); + meshBuilder.addSphere(0.03f, comDesired, Color.GREEN); + + desiredCoMXYAtFootHeight.setIncludingFrame(comDesired); + desiredCoMXYAtFootHeight.setZ(footZ); + meshBuilder.addSphere(0.03f, desiredCoMXYAtFootHeight, Color.GREEN); - Color color = i == minimumEdgeIndex ? Color.RED : POLYGON_GRAPHIC_COLOR; - meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color); + FramePoint3D desiredComXYElevated = new FramePoint3D(desiredCoMXYAtFootHeight); + desiredComXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); + meshBuilder.addLine(desiredCoMXYAtFootHeight, desiredComXYElevated, 0.005f, Color.GREEN); + } } - meshBuilder.addPolygon(transform, multiContactSupportRegion, POLYGON_GRAPHIC_COLOR); + // Update region graphic + Object ikSupportRegion = kinematicsToolboxOutputStatus.getSupportRegion(); + if (ikSupportRegion.size() >= 3) + { + this.supportRegion.clear(); + + for (int i = 0; i < ikSupportRegion.size(); i++) + { + this.supportRegion.addVertex(ikSupportRegion.get(i)); + } + + this.supportRegion.update(); - comCurrent.setToZero(centerOfMassFrame); - comCurrent.changeFrame(ReferenceFrame.getWorldFrame()); + if (this.supportRegion.getNumberOfVertices() < 3) + { + modelInstance = null; + lastModel = null; + return; + } - meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK); + updateMinimumEdge(); - comXYAtFootHeight.setIncludingFrame(comCurrent); - comXYAtFootHeight.setZ(footZ); - meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK); + transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ); - FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight); - comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); - meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK); + for (int i = 0; i < this.supportRegion.getNumberOfVertices(); i++) + { + Point2DReadOnly v0 = this.supportRegion.getVertex(i); + Point2DReadOnly v1 = this.supportRegion.getNextVertex(i); + + Color color = i == minimumEdgeIndex ? Color.RED : NOMINAL_POLYGON_GRAPHIC_COLOR; + meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color); + } + + double postureSensitivityThreshold = 0.045; + double stabilityMarginThreshold = 0.12; + + boolean isPostureHighlySensitive = kinematicsToolboxOutputStatus.getSupportRegionSensitivity() > postureSensitivityThreshold; + boolean hasLowStabilityMargin = minimumEdgeDistance < stabilityMarginThreshold; + + Color polygonGraphicColor = (isPostureHighlySensitive && hasLowStabilityMargin) ? LOW_STABILITY_POLYGON_GRAPHIC_COLOR : NOMINAL_POLYGON_GRAPHIC_COLOR; + meshBuilder.addPolygon(transform, this.supportRegion, polygonGraphicColor); + } modelBuilder.begin(); Mesh mesh = meshBuilder.generateMesh(); @@ -139,10 +169,10 @@ private void updateMinimumEdge() { minimumEdgeDistance = Double.POSITIVE_INFINITY; - for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++) + for (int i = 0; i < supportRegion.getNumberOfVertices(); i++) { - Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i); - Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i); + Point2DReadOnly v0 = supportRegion.getVertex(i); + Point2DReadOnly v1 = supportRegion.getNextVertex(i); double margin = EuclidGeometryTools.distanceFromPoint2DToLine2D(comXYAtFootHeight.getX(), comXYAtFootHeight.getY(), v0, v1); if (margin < minimumEdgeDistance) @@ -156,7 +186,7 @@ private void updateMinimumEdge() @Override public void getRenderables(Array renderables, Pool pool) { - if (multiContactSupportRegion.isEmpty()) + if (supportRegion.isEmpty()) { return; } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java index 1ef2012adf8..694bb86ec08 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java @@ -745,7 +745,7 @@ else if (latestStatus.getCurrentToolboxState() == KinematicsToolboxOutputStatus. ghostOneDoFJointsExcludingHands[i].setQ(latestStatus.getDesiredJointAngles().get(i)); } ghostFullRobotModel.getElevator().updateFramesRecursively(); - multiContactStabilityGraphic.update(latestStatus); + multiContactStabilityGraphic.update(latestStatus, null); } } if (capturabilityBasedStatus.getMessageNotification().poll()) diff --git a/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl b/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl index 4f8cba84bf7..efb942202d9 100644 --- a/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl +++ b/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl @@ -80,6 +80,11 @@ module toolbox_msgs * Support region used by the toolbox */ sequence support_region; + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + @defaultValue(value=-1.0) + double support_region_sensitivity; @defaultValue(value=-1.0) double solution_quality; }; diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java index 7075835bb13..156e0556653 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class KinematicsPlanningToolboxOutputStatusPubSubType implements us.ihmc. @Override public final java.lang.String getDefinitionChecksum() { - return "a503326897dee5045f25aa53a759d78ab1301ea0b64e652c49ff3c85ee497579"; + return "b90a1333b8ecfd4b7d1781eb80ab2337d331638dd4ce7bd34568aab14a7c8ddb"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java index 7c74f7ca6c5..9cc71efd8f7 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java @@ -66,6 +66,10 @@ public class KinematicsToolboxOutputStatus extends Packet support_region_; + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public double support_region_sensitivity_ = -1.0; public double solution_quality_ = -1.0; public KinematicsToolboxOutputStatus() @@ -103,6 +107,8 @@ public void set(KinematicsToolboxOutputStatus other) geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.desired_root_linear_velocity_, desired_root_linear_velocity_); geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.desired_root_angular_velocity_, desired_root_angular_velocity_); support_region_.set(other.support_region_); + support_region_sensitivity_ = other.support_region_sensitivity_; + solution_quality_ = other.solution_quality_; } @@ -209,6 +215,21 @@ public us.ihmc.idl.IDLSequence.Object getSuppor return support_region_; } + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public void setSupportRegionSensitivity(double support_region_sensitivity) + { + support_region_sensitivity_ = support_region_sensitivity; + } + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public double getSupportRegionSensitivity() + { + return support_region_sensitivity_; + } + public void setSolutionQuality(double solution_quality) { solution_quality_ = solution_quality; @@ -257,6 +278,8 @@ public boolean epsilonEquals(KinematicsToolboxOutputStatus other, double epsilon { if (!this.support_region_.get(i).epsilonEquals(other.support_region_.get(i), epsilon)) return false; } } + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.support_region_sensitivity_, other.support_region_sensitivity_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.solution_quality_, other.solution_quality_, epsilon)) return false; @@ -285,6 +308,8 @@ public boolean equals(Object other) if (!this.desired_root_linear_velocity_.equals(otherMyClass.desired_root_linear_velocity_)) return false; if (!this.desired_root_angular_velocity_.equals(otherMyClass.desired_root_angular_velocity_)) return false; if (!this.support_region_.equals(otherMyClass.support_region_)) return false; + if(this.support_region_sensitivity_ != otherMyClass.support_region_sensitivity_) return false; + if(this.solution_quality_ != otherMyClass.solution_quality_) return false; @@ -317,6 +342,8 @@ public java.lang.String toString() builder.append(this.desired_root_angular_velocity_); builder.append(", "); builder.append("support_region="); builder.append(this.support_region_); builder.append(", "); + builder.append("support_region_sensitivity="); + builder.append(this.support_region_sensitivity_); builder.append(", "); builder.append("solution_quality="); builder.append(this.solution_quality_); builder.append("}"); diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java index 9c8e961a1db..2a9eaad7caa 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class KinematicsToolboxOutputStatusPubSubType implements us.ihmc.pubsub.T @Override public final java.lang.String getDefinitionChecksum() { - return "624cc92872f1d8473b47e098815669b6fb9c0a5c2459a80441afc9913a518f73"; + return "24e1c048ab9f7fc5b8bb75a4b51d96efd5aad483599cdaba27efc225e88dbebd"; } @Override @@ -75,6 +75,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);} current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + return current_alignment - initial_alignment; } @@ -121,6 +123,9 @@ public final static int getCdrSerializedSize(toolbox_msgs.msg.dds.KinematicsTool current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + return current_alignment - initial_alignment; } @@ -149,6 +154,8 @@ public static void write(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus data cdr.write_type_e(data.getSupportRegion());else throw new RuntimeException("support_region field exceeds the maximum length"); + cdr.write_type_6(data.getSupportRegionSensitivity()); + cdr.write_type_6(data.getSolutionQuality()); } @@ -168,6 +175,8 @@ public static void read(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus data, geometry_msgs.msg.dds.Vector3PubSubType.read(data.getDesiredRootLinearVelocity(), cdr); geometry_msgs.msg.dds.Vector3PubSubType.read(data.getDesiredRootAngularVelocity(), cdr); cdr.read_type_e(data.getSupportRegion()); + data.setSupportRegionSensitivity(cdr.read_type_6()); + data.setSolutionQuality(cdr.read_type_6()); @@ -190,6 +199,7 @@ public final void serialize(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus d ser.write_type_a("desired_root_angular_velocity", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getDesiredRootAngularVelocity()); ser.write_type_e("support_region", data.getSupportRegion()); + ser.write_type_6("support_region_sensitivity", data.getSupportRegionSensitivity()); ser.write_type_6("solution_quality", data.getSolutionQuality()); } @@ -210,6 +220,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, toolbox_msg ser.read_type_a("desired_root_angular_velocity", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getDesiredRootAngularVelocity()); ser.read_type_e("support_region", data.getSupportRegion()); + data.setSupportRegionSensitivity(ser.read_type_6("support_region_sensitivity")); data.setSolutionQuality(ser.read_type_6("solution_quality")); } diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java index 60485605362..44a60f62808 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java @@ -15,7 +15,7 @@ public class WalkingControllerPreviewOutputMessagePubSubType implements us.ihmc. @Override public final java.lang.String getDefinitionChecksum() { - return "85f7193434b41d5e0acd89bb0f6e673559fd4d1937cb356ec8c9f5ba50ea4987"; + return "88099551935668d5c7e7176614054177319b76eb9daa4ed8e46971b48e26f2b8"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java index 404a923707a..bf720262363 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxConfigurationMessagePubSubType implements @Override public final java.lang.String getDefinitionChecksum() { - return "31e206ec5e6f4cb3bcc48761075d11243576488d1865e110f3a90183f043f2ee"; + return "eaee35e911cc5f906d7a1175653f936458205629c5b723a1196566e05a5ee4d4"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java index d85b7694b2a..9a96363140a 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxMessagePubSubType implements us.ihmc.pubs @Override public final java.lang.String getDefinitionChecksum() { - return "0507fa389750db026c3d95bc6976be87f4dec98a0413bc67f760b93e7438cf70"; + return "785094e2646fca4937cca843355b88163fd8518cae80ba813ab6247a83dddedd"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java index 81582744d25..3ac539a27d7 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxOutputStatusPubSubType implements us.ihmc @Override public final java.lang.String getDefinitionChecksum() { - return "a862a03a5fd7d21b9fce0b609911614c4abcb5d99da934102fd2d484b5356e48"; + return "2b8e6d1b94f9dfc4a0f3d2d6128d9a546cb73c0326c939e04ec1d8e0f5c9b081"; } @Override diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg index 2abce879699..8c211c0eccf 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg @@ -38,4 +38,7 @@ geometry_msgs/Vector3 desired_root_angular_velocity # Support region used by the toolbox geometry_msgs/Point[<=32] support_region +# Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. +float64 support_region_sensitivity -1 + float64 solution_quality -1 \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg b/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg index d6f0fb2795d..9c7d2a7af49 100644 --- a/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg +++ b/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg @@ -43,6 +43,10 @@ geometry_msgs/Vector3 desired_root_angular_velocity # Support region used by the toolbox geometry_msgs/Point[] support_region +# Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. +# Field default value -1.0 +float64 support_region_sensitivity + # Field default value -1.0 float64 solution_quality