Skip to content

KST visualization #656

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: develop
Choose a base branch
from
Open

KST visualization #656

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
17012df
Adding KST param to deactivate joints, similar to controller
stephenmcc Feb 28, 2025
fe13cc2
Publishing com region and sensitivity by default and adding correspon…
stephenmcc Feb 11, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 7 additions & 6 deletions .../avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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<Point3D> leftFootSupportPolygon2d = capturabilityBasedStatus.getLeftFootSupportPolygon3d();
Object<Point3D> rightFootSupportPolygon2d = capturabilityBasedStatus.getRightFootSupportPolygon3d();
for (int i = 0; i < leftFootSupportPolygon2d.size(); i++)
Expand Down Expand Up @@ -813,7 +812,9 @@ protected void getAdditionalFeedbackControlCommands(FeedbackControlCommandBuffer
{
addHoldSupportEndEffectorCommands(bufferToPack);
addHoldCenterOfMassXYCommand(bufferToPack);
stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack);

double supportRegionSensitivity = stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack);
getSolution().setSupportRegionSensitivity(supportRegionSensitivity);
}

@Override
Expand Down
36 changes: 24 additions & 12 deletions ...tar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand All @@ -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();
Expand Down Expand Up @@ -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;
}
}
7 changes: 7 additions & 0 deletions ...avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -224,6 +224,13 @@ public IKStreamingRTThread(String robotName,
fullRobotModelFactory,
yoGraphicsListRegistry,
registry);

List<String> inactiveJoints = parameters.getInactiveJoints();
for (int i = 0; i < inactiveJoints.size(); i++)
{
kinematicsStreamingToolboxController.getActiveOptimizationSettings().deactivateJoint(desiredFullRobotModel.getOneDoFJointByName(inactiveJoints.get(i)));
}

kinematicsStreamingToolboxController.setCollisionModel(collisionModel);

MessageUnpacker<WholeBodyStreamingMessage> wholeBodyStreamingMessageUnpacker = MessageUnpackingTools.createWholeBodyStreamingMessageUnpacker();
Expand Down
6 changes: 6 additions & 0 deletions ...workProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
5 changes: 5 additions & 0 deletions .../networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,11 @@ public KinematicsStreamingToolboxModule(DRCRobotModel robotModel,
registry);
controller.setRobotStateUpdater(robotStateUpdater);
controller.setCollisionModel(robotModel.getHumanoidRobotKinematicsCollisionModel());
List<String> inactiveJoints = parameters.getInactiveJoints();
for (int i = 0; i < inactiveJoints.size(); i++)
{
controller.getActiveOptimizationSettings().deactivateJoint(controller.getDesiredFullRobotModel().getOneDoFJointByName(inactiveJoints.get(i)));
}
Map<String, Double> initialConfiguration = fromStandPrep(robotModel);
if (initialConfiguration != null)
controller.setInitialRobotConfigurationNamedMap(initialConfiguration);
Expand Down
9 changes: 9 additions & 0 deletions ...workProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -269,6 +271,8 @@ public enum ClockType
private double solverPrivilegedDefaultWeight;
private double solverPrivilegedDefaultGain;

private final List<String> inactiveJoints = new ArrayList<>();

public static KinematicsStreamingToolboxParameters defaultParameters()
{
KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters();
Expand Down Expand Up @@ -1059,4 +1063,9 @@ public void setSolverPrivilegedDefaultGain(double solverPrivilegedDefaultGain)
{
this.solverPrivilegedDefaultGain = solverPrivilegedDefaultGain;
}

public List<String> getInactiveJoints()
{
return inactiveJoints;
}
}
120 changes: 75 additions & 45 deletions ...level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -55,68 +56,97 @@ 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(),
ghostFullRobotModel.getSoleFrame(RobotSide.LEFT),
ghostFullRobotModel.getSoleFrame(RobotSide.RIGHT));
}

public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus)
public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus, FramePoint3D desiredCoMPosition)
{
multiContactSupportRegion.clear();
Object<Point3D> 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<Point3D> 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();
Expand All @@ -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)
Expand All @@ -156,7 +186,7 @@ private void updateMinimumEdge()
@Override
public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
{
if (multiContactSupportRegion.isEmpty())
if (supportRegion.isEmpty())
{
return;
}
Expand Down
2 changes: 1 addition & 1 deletion ...-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand Down
Loading
Loading