Skip to content

Bugfix/fix capture region and rate limit #1110

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

Merged
merged 2 commits into from
Nov 18, 2025
Merged

Bugfix/fix capture region and rate limit #1110

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
4480ce4
fixed the reachability constraint return
rjgriffin42 Nov 18, 2025
eb92a54
properly reset the feedback conditions for the ICP feedback
rjgriffin42 Nov 18, 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
8 changes: 6 additions & 2 deletions .../main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -417,8 +417,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly

UnrolledInverseFromMinor_DDRM.inv(transformedGains, inverseTransformedGains);

solver.resetCoPFeedbackConditions();
solver.resetFeedbackDirection();
solver.resetFeedbackConditions();
Copy link
Member Author

@rjgriffin42 rjgriffin42 Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I consolidated this logic into a single reset method, while also removing the limits.

solver.setFeedbackConditions(scaledCoPFeedbackWeight, transformedGains, dynamicsObjectiveWeight.getValue());
solver.setMaxCMPDistanceFromEdge(maxAllowedDistanceCMPSupport.getValue());
solver.setCopSafeDistanceToEdge(safeCoPDistanceToEdge.getValue());
Expand All @@ -442,6 +441,11 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly

solver.setMaximumFeedbackRate(feedbackGains.getFeedbackPartMaxRate(), controlDT);
}
else
{
solver.removeMaximumFeedbackMagnitude();
solver.removeFeedbackRateLimit();
}
Copy link
Member Author

@rjgriffin42 rjgriffin42 Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Previously, what happened is that we wouldn't add the magnitude limits or rate limits if it was in a reset. However, those values were never getting reset inside the ICPControllerQPSolver. So it was applying whatever they were the last time they were set. So we were never actually removing these constraints.

ignoreRateLimitThisTick = false;

double feedbackRateWeight = usingHighCoPDamping.getValue() ? highlyDampedFeedbackRateWeight.getValue() : this.feedbackRateWeight.getValue();
Expand Down
21 changes: 21 additions & 0 deletions ...va/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -294,6 +294,18 @@ public void setMaximumFeedbackRate(double maximumFeedbackRate, double controlDT)
this.controlDT = controlDT;
}

public void removeFeedbackRateLimit()
{
this.maximumFeedbackRate = Double.POSITIVE_INFINITY;
}

public void removeMaximumFeedbackMagnitude()
{
CommonOps_DDRM.setIdentity(maxFeedbackTransform);
maxFeedbackXMagnitude = Double.POSITIVE_INFINITY;
maxFeedbackYMagnitude = Double.POSITIVE_INFINITY;
}

/**
* Zeros all the pertinent scalars, vectors, and matrices for the solver. Should be called at the beginning of every computation tick.
*/
Expand Down Expand Up @@ -357,6 +369,15 @@ private void reshape()
solution.reshape(problemSize, 1);
}

public void resetFeedbackConditions()
{
resetCoPFeedbackConditions();
resetCMPFeedbackConditions();
resetFeedbackDirection();
removeFeedbackRateLimit();
removeMaximumFeedbackMagnitude();
Copy link
Member Author

@rjgriffin42 rjgriffin42 Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we were never calling "resetCMPFeedbackConditions". While this didn't cause any problems, it is a bug. The other two are the things that reset those rate limits and magnitude limits each time so they can be selectively enforced.

}

/**
* Resets the controller conditions on the feedback minimization task, the feedback gains, and the dynamic relaxation minimization task.
* Also sets that the controller is not to attempt to regularize the feedback minimization task.
Expand Down
19 changes: 15 additions & 4 deletions ...WalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -448,7 +448,7 @@ public void compute(double currentTime,
{
tempPoint2D.set(footstepSolution.getPosition());
computeBestReachabilityConstraintToUseWhenNotIntersecting();
FrameConvexPolygon2DReadOnly reachability = getSelectedRegion();
FrameConvexPolygon2DReadOnly reachability = getSelectedReachableRegion();
if (!reachability.isPointInside(tempPoint2D))
reachability.orthogonalProjection(tempPoint2D);
footstepSolution.getPosition().set(tempPoint2D);
Expand Down Expand Up @@ -559,12 +559,12 @@ private void projectAdjustedStepIntoCaptureRegion(FramePoint3DReadOnly pointToPr
{
captureRegionInWorld.orthogonalProjection(adjustedSolution);
computeBestReachabilityConstraintToUseWhenNotIntersecting();
getSelectedRegion().orthogonalProjection(adjustedSolution);
getSelectedReachableRegion().orthogonalProjection(adjustedSolution);
}
else
{
computeBestReachabilityConstraintToUseWhenIntersecting();
getSelectedRegion().orthogonalProjection(adjustedSolution);
Copy link
Member Author

@rjgriffin42 rjgriffin42 Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I switched this back to what it was previously before the previous change. As written in the code, if returning the selected region, it returned the intersection between the reachable region and the capture point. Previously, the conditions would return this if the intersections were valid, so that there was an intersection, and if there were no intersections, it would return just the reachable region. The change in the PR returned only the selected one, so that the projection failed.

I changed this, so that if there's an intersection, it returns the selected reachable region, while refactoring the previous method to say "selectedReachableCaptureRegion", which is what is returned if these regions are valid.

getSelectedReachableCaptureRegion().orthogonalProjection(adjustedSolution);
}

footstepAdjustment.set(adjustedSolution);
Expand Down Expand Up @@ -706,7 +706,7 @@ private double getSelectedArea()
};
}

private FrameConvexPolygon2DReadOnly getSelectedRegion()
private FrameConvexPolygon2DReadOnly getSelectedReachableCaptureRegion()
{
return switch (selectedReachableRegion.getEnumValue())
{
Expand All @@ -716,6 +716,17 @@ private FrameConvexPolygon2DReadOnly getSelectedRegion()
};
}

private FrameConvexPolygon2DReadOnly getSelectedReachableRegion()
{
return switch (selectedReachableRegion.getEnumValue())
{
case FORWARD -> reachabilityConstraintHandler.getForwardCrossOverPolygon();
case BACKWARD -> reachabilityConstraintHandler.getBackwardCrossOverPolygon();
case BASELINE -> reachabilityConstraintHandler.getReachabilityConstraint();
};
}


private boolean deadbandAndApplyStepAdjustment()
{
boolean adjusted;
Expand Down
Loading