Skip to content

Commit fe13cc2

Browse files
stephenmccstephenmcc
committed
Publishing com region and sensitivity by default and adding corresponding visualizers
1 parent 17012df commit fe13cc2

14 files changed

+163
-70
lines changed

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

+7-6
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,6 @@
6969
public class HumanoidKinematicsToolboxController extends KinematicsToolboxController
7070
{
7171
private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
72-
private static final Vector3D zeroVector = new Vector3D();
7372
private static final double FOOT_COEFFICIENT_OF_FRICTION = 0.8;
7473
private static final double HAND_COEFFICIENT_OF_FRICTION = 0.4;
7574

@@ -245,6 +244,9 @@ public HumanoidKinematicsToolboxController(CommandInputManager commandInputManag
245244
setupVisualization(desiredFullRobotModel.getFoot(robotSide));
246245
}
247246

247+
setupVisualization(desiredFullRobotModel.getPelvis());
248+
setupVisualization(desiredFullRobotModel.getChest());
249+
248250
for (RobotSide robotSide : RobotSide.values)
249251
{
250252
String side = robotSide.getCamelCaseNameForMiddleOfExpression();
@@ -529,14 +531,10 @@ public void updateInternal()
529531
if (multiContactRegionCalculator.hasSolvedWholeRegion())
530532
{
531533
activeContactPointPositions.clear();
532-
getSolution().getSupportRegion().clear();
533-
534534
for (int i = 0; i < multiContactRegionCalculator.getNumberOfVertices(); i++)
535535
{
536536
activeContactPointPositions.add().set(multiContactRegionCalculator.getOptimizedVertex(i));
537-
getSolution().getSupportRegion().add().set(multiContactRegionCalculator.getOptimizedVertex(i));
538537
}
539-
540538
updateSupportPolygonConstraint(activeContactPointPositions);
541539
}
542540
}
@@ -733,6 +731,7 @@ private void processCapturabilityBasedStatus(CapturabilityBasedStatus capturabil
733731
}
734732

735733
// CoM constraint polygon is the convex hull of the feet contact points. Even when upper body is load-bearing, initialize to this.
734+
activeContactPointPositions.clear();
736735
Object<Point3D> leftFootSupportPolygon2d = capturabilityBasedStatus.getLeftFootSupportPolygon3d();
737736
Object<Point3D> rightFootSupportPolygon2d = capturabilityBasedStatus.getRightFootSupportPolygon3d();
738737
for (int i = 0; i < leftFootSupportPolygon2d.size(); i++)
@@ -813,7 +812,9 @@ protected void getAdditionalFeedbackControlCommands(FeedbackControlCommandBuffer
813812
{
814813
addHoldSupportEndEffectorCommands(bufferToPack);
815814
addHoldCenterOfMassXYCommand(bufferToPack);
816-
stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack);
815+
816+
double supportRegionSensitivity = stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack);
817+
getSolution().setSupportRegionSensitivity(supportRegionSensitivity);
817818
}
818819

819820
@Override

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

+24-12
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
public class StabilityMarginKinematicsCostCalculator
2626
{
2727
private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
28+
private static final double NULL_POSTURAL_SENSITIVITY = -1.0;
2829

2930
private final WholeBodyContactState wholeBodyContactState;
3031
private final StabilityMarginRegionCalculator multiContactRegionCalculator;
@@ -65,7 +66,10 @@ public StabilityMarginKinematicsCostCalculator(WholeBodyContactState wholeBodyCo
6566
pelvisControlFramePose.setToZero(afterRootJointFrame);
6667
pelvisControlFramePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame());
6768

68-
this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel, wholeBodyContactState, multiContactRegionCalculator, registry);
69+
this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel,
70+
wholeBodyContactState,
71+
multiContactRegionCalculator,
72+
registry);
6973
parentRegistry.addChild(registry);
7074
}
7175

@@ -74,26 +78,32 @@ public void setEnabled(boolean enable)
7478
isEnabled.set(enable);
7579
}
7680

77-
public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack)
81+
public boolean isEnabled()
7882
{
79-
if (!isEnabled.getBooleanValue() || !isUpperBodyLoadBearing.getValue())
80-
return;
83+
return isEnabled.getBooleanValue();
84+
}
8185

82-
double stabilityMargin = multiContactRegionCalculator.getStabilityMargin();
83-
if (stabilityMargin > maxMarginToPenalize.getValue())
84-
return;
86+
/**
87+
* Computes and packs the feedback objective. Returns the postural sensitivity
88+
*/
89+
public double addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack)
90+
{
91+
if (!isUpperBodyLoadBearing.getValue() || !multiContactRegionCalculator.hasSolvedWholeRegion())
92+
return NULL_POSTURAL_SENSITIVITY;
8593

94+
double stabilityMargin = multiContactRegionCalculator.getStabilityMargin();
8695
double deltaStabilityMargin = stabilityMargin - minStabilityMargin.getValue();
8796
double alpha = EuclidCoreTools.clamp(deltaStabilityMargin / minStabilityMargin.getValue(), 0.0, 1.0);
8897
double weight = alpha * stabilityMarginWeight.getValue();
8998

9099
stabilityGradientCalculator.update();
91-
DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient();
92-
double stabilityGradientMagnitude = CommonOps_DDRM.dot(stabilityMarginGradient, stabilityMarginGradient);
93-
if (stabilityGradientMagnitude < 1.0e-5)
94-
return;
100+
double posturalSensitivity = stabilityGradientCalculator.getPostureSensitivity();
101+
102+
if (!isEnabled.getValue() || posturalSensitivity < 1.0e-3)
103+
return posturalSensitivity;
95104

96-
double gradientScalar = desiredStabilityMarginVelocity.getValue() / stabilityGradientMagnitude;
105+
DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient();
106+
double gradientScalar = desiredStabilityMarginVelocity.getValue() / EuclidCoreTools.square(posturalSensitivity);
97107

98108
// Feed-forward joint velocities
99109
OneDoFJointBasics[] oneDoFJoints = wholeBodyContactState.getOneDoFJoints();
@@ -131,5 +141,7 @@ public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack
131141
spatialFeedbackControlCommand.getGains().getPositionGains().setDerivativeGains(0.0);
132142
spatialFeedbackControlCommand.getGains().getOrientationGains().setProportionalGains(0.0);
133143
spatialFeedbackControlCommand.getGains().getOrientationGains().setDerivativeGains(0.0);
144+
145+
return posturalSensitivity;
134146
}
135147
}

‎ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java

+75-45
Original file line numberDiff line numberDiff line change
@@ -31,21 +31,22 @@
3131

3232
public class RDXMultiContactRegionGraphic implements RenderableProvider
3333
{
34-
private static final Color POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933");
34+
private static final Color NOMINAL_POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933");
35+
private static final Color LOW_STABILITY_POLYGON_GRAPHIC_COLOR = Color.valueOf("EB3D40");
3536
private static final double STABILITY_GRAPHIC_HEIGHT = 2.0;
3637

37-
private final ConvexPolygon2D multiContactSupportRegion = new ConvexPolygon2D();
38+
private final ConvexPolygon2D supportRegion = new ConvexPolygon2D();
3839

3940
private final FramePoint3D comCurrent = new FramePoint3D();
41+
private final FramePoint3D comDesired = new FramePoint3D();
4042
private final FramePoint3D comXYAtFootHeight = new FramePoint3D();
41-
private final ConvexPolygon2D closestProximityEdge = new ConvexPolygon2D();
43+
private final FramePoint3D desiredCoMXYAtFootHeight = new FramePoint3D();
4244

4345
private int minimumEdgeIndex;
4446
private double minimumEdgeDistance;
4547

4648
private final ModelBuilder modelBuilder = new ModelBuilder();
4749
private final RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder();
48-
private final FullHumanoidRobotModel ghostFullRobotModel;
4950
private final CenterOfMassReferenceFrame centerOfMassFrame;
5051
private final MidFrameZUpFrame midFeetZUpFrame;
5152
private final RigidBodyTransform transform = new RigidBodyTransform();
@@ -55,68 +56,97 @@ public class RDXMultiContactRegionGraphic implements RenderableProvider
5556

5657
public RDXMultiContactRegionGraphic(FullHumanoidRobotModel ghostFullRobotModel)
5758
{
58-
this.ghostFullRobotModel = ghostFullRobotModel;
5959
this.centerOfMassFrame = new CenterOfMassReferenceFrame("ghostCoMFrame", ReferenceFrame.getWorldFrame(), ghostFullRobotModel.getRootBody());
6060
this.midFeetZUpFrame = new MidFrameZUpFrame("midFeedZUpWhost",
6161
ReferenceFrame.getWorldFrame(),
6262
ghostFullRobotModel.getSoleFrame(RobotSide.LEFT),
6363
ghostFullRobotModel.getSoleFrame(RobotSide.RIGHT));
6464
}
6565

66-
public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus)
66+
public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus, FramePoint3D desiredCoMPosition)
6767
{
68-
multiContactSupportRegion.clear();
69-
Object<Point3D> supportRegion = kinematicsToolboxOutputStatus.getSupportRegion();
70-
71-
for (int i = 0; i < supportRegion.size(); i++)
72-
{
73-
multiContactSupportRegion.addVertex(supportRegion.get(i));
74-
}
75-
76-
multiContactSupportRegion.update();
77-
78-
if (multiContactSupportRegion.getNumberOfVertices() < 3)
79-
{
80-
modelInstance = null;
81-
lastModel = null;
82-
return;
83-
}
84-
8568
meshBuilder.clear();
8669

70+
// Update frames and compute mid-feet height
8771
centerOfMassFrame.update();
8872
midFeetZUpFrame.update();
89-
updateMinimumEdge();
9073

9174
FramePoint3D midFoot = new FramePoint3D(midFeetZUpFrame);
9275
midFoot.changeFrame(ReferenceFrame.getWorldFrame());
9376
double footZ = midFoot.getZ();
9477

95-
transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ);
96-
97-
for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++)
78+
// Update desired and achieved CoM graphic
9879
{
99-
Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i);
100-
Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i);
80+
comCurrent.setToZero(centerOfMassFrame);
81+
comCurrent.changeFrame(ReferenceFrame.getWorldFrame());
82+
83+
meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK);
84+
85+
comXYAtFootHeight.setIncludingFrame(comCurrent);
86+
comXYAtFootHeight.setZ(footZ);
87+
meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK);
88+
89+
FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight);
90+
comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT);
91+
meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK);
92+
93+
if (desiredCoMPosition != null)
94+
{
95+
comDesired.setMatchingFrame(desiredCoMPosition);
96+
meshBuilder.addSphere(0.03f, comDesired, Color.GREEN);
97+
98+
desiredCoMXYAtFootHeight.setIncludingFrame(comDesired);
99+
desiredCoMXYAtFootHeight.setZ(footZ);
100+
meshBuilder.addSphere(0.03f, desiredCoMXYAtFootHeight, Color.GREEN);
101101

102-
Color color = i == minimumEdgeIndex ? Color.RED : POLYGON_GRAPHIC_COLOR;
103-
meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color);
102+
FramePoint3D desiredComXYElevated = new FramePoint3D(desiredCoMXYAtFootHeight);
103+
desiredComXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT);
104+
meshBuilder.addLine(desiredCoMXYAtFootHeight, desiredComXYElevated, 0.005f, Color.GREEN);
105+
}
104106
}
105107

106-
meshBuilder.addPolygon(transform, multiContactSupportRegion, POLYGON_GRAPHIC_COLOR);
108+
// Update region graphic
109+
Object<Point3D> ikSupportRegion = kinematicsToolboxOutputStatus.getSupportRegion();
110+
if (ikSupportRegion.size() >= 3)
111+
{
112+
this.supportRegion.clear();
113+
114+
for (int i = 0; i < ikSupportRegion.size(); i++)
115+
{
116+
this.supportRegion.addVertex(ikSupportRegion.get(i));
117+
}
118+
119+
this.supportRegion.update();
107120

108-
comCurrent.setToZero(centerOfMassFrame);
109-
comCurrent.changeFrame(ReferenceFrame.getWorldFrame());
121+
if (this.supportRegion.getNumberOfVertices() < 3)
122+
{
123+
modelInstance = null;
124+
lastModel = null;
125+
return;
126+
}
110127

111-
meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK);
128+
updateMinimumEdge();
112129

113-
comXYAtFootHeight.setIncludingFrame(comCurrent);
114-
comXYAtFootHeight.setZ(footZ);
115-
meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK);
130+
transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ);
116131

117-
FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight);
118-
comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT);
119-
meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK);
132+
for (int i = 0; i < this.supportRegion.getNumberOfVertices(); i++)
133+
{
134+
Point2DReadOnly v0 = this.supportRegion.getVertex(i);
135+
Point2DReadOnly v1 = this.supportRegion.getNextVertex(i);
136+
137+
Color color = i == minimumEdgeIndex ? Color.RED : NOMINAL_POLYGON_GRAPHIC_COLOR;
138+
meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color);
139+
}
140+
141+
double postureSensitivityThreshold = 0.045;
142+
double stabilityMarginThreshold = 0.12;
143+
144+
boolean isPostureHighlySensitive = kinematicsToolboxOutputStatus.getSupportRegionSensitivity() > postureSensitivityThreshold;
145+
boolean hasLowStabilityMargin = minimumEdgeDistance < stabilityMarginThreshold;
146+
147+
Color polygonGraphicColor = (isPostureHighlySensitive && hasLowStabilityMargin) ? LOW_STABILITY_POLYGON_GRAPHIC_COLOR : NOMINAL_POLYGON_GRAPHIC_COLOR;
148+
meshBuilder.addPolygon(transform, this.supportRegion, polygonGraphicColor);
149+
}
120150

121151
modelBuilder.begin();
122152
Mesh mesh = meshBuilder.generateMesh();
@@ -139,10 +169,10 @@ private void updateMinimumEdge()
139169
{
140170
minimumEdgeDistance = Double.POSITIVE_INFINITY;
141171

142-
for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++)
172+
for (int i = 0; i < supportRegion.getNumberOfVertices(); i++)
143173
{
144-
Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i);
145-
Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i);
174+
Point2DReadOnly v0 = supportRegion.getVertex(i);
175+
Point2DReadOnly v1 = supportRegion.getNextVertex(i);
146176

147177
double margin = EuclidGeometryTools.distanceFromPoint2DToLine2D(comXYAtFootHeight.getX(), comXYAtFootHeight.getY(), v0, v1);
148178
if (margin < minimumEdgeDistance)
@@ -156,7 +186,7 @@ private void updateMinimumEdge()
156186
@Override
157187
public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
158188
{
159-
if (multiContactSupportRegion.isEmpty())
189+
if (supportRegion.isEmpty())
160190
{
161191
return;
162192
}

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -745,7 +745,7 @@ else if (latestStatus.getCurrentToolboxState() == KinematicsToolboxOutputStatus.
745745
ghostOneDoFJointsExcludingHands[i].setQ(latestStatus.getDesiredJointAngles().get(i));
746746
}
747747
ghostFullRobotModel.getElevator().updateFramesRecursively();
748-
multiContactStabilityGraphic.update(latestStatus);
748+
multiContactStabilityGraphic.update(latestStatus, null);
749749
}
750750
}
751751
if (capturabilityBasedStatus.getMessageNotification().poll())

‎ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl

+5
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,11 @@ module toolbox_msgs
8080
* Support region used by the toolbox
8181
*/
8282
sequence<geometry_msgs::msg::dds::Point, 32> support_region;
83+
/**
84+
* Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective.
85+
*/
86+
@defaultValue(value=-1.0)
87+
double support_region_sensitivity;
8388
@defaultValue(value=-1.0)
8489
double solution_quality;
8590
};

‎ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public class KinematicsPlanningToolboxOutputStatusPubSubType implements us.ihmc.
1515
@Override
1616
public final java.lang.String getDefinitionChecksum()
1717
{
18-
return "a503326897dee5045f25aa53a759d78ab1301ea0b64e652c49ff3c85ee497579";
18+
return "b90a1333b8ecfd4b7d1781eb80ab2337d331638dd4ce7bd34568aab14a7c8ddb";
1919
}
2020

2121
@Override

0 commit comments

Comments
 (0)