private FramePoint2d closestPoint(FramePoint2d point, FramePoint2d candidate1, FramePoint2d candidate2) { if (candidate1.containsNaN() && candidate2.containsNaN()) return null; if (candidate1.containsNaN()) return candidate2; if (candidate2.containsNaN()) return candidate1; if (point.distance(candidate1) <= point.distance(candidate2)) return candidate1; return candidate2; }
@Override public boolean containsNaN() { return (position.containsNaN() || orientation.containsNaN()); }
private void intersectASinglePolygon(FrameConvexPolygon2d polygon, Pair<FramePoint, FramePoint> intersectionWithPolygon) { intersectionOfPlanes.changeFrame(polygon.getReferenceFrame()); intersectionOfPlanes.projectOntoXYPlane(planeIntersectionOnPolygonPlane); polygon.intersectionWith(planeIntersectionOnPolygonPlane, lineIntersectionOnPolygonPlane); if (lineIntersectionOnPolygonPlane.getFirst().containsNaN() && lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { noIntersection = true; } if (!lineIntersectionOnPolygonPlane.getFirst().containsNaN()) { intersectionWithPolygon.getFirst().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getFirst()); } if (!lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { intersectionWithPolygon.getSecond().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getSecond()); } }
public void update() { momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); if (desiredCoP.containsNaN()) isDesiredCoPOnEdge.set(false); else { double epsilon = isDesiredCoPOnEdge.getBooleanValue() ? EPSILON_POINT_ON_EDGE_WITH_HYSTERESIS : EPSILON_POINT_ON_EDGE; FrameConvexPolygon2d footSupportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(robotSide); isDesiredCoPOnEdge.set(!footSupportPolygon.isPointInside(desiredCoP, -epsilon)); // Minus means that the check is done with a smaller polygon } }
footSwitches.get(trustedFoot).computeAndPackCoP(tempCoP2d); if (tempCoP2d.containsNaN())
if (firstEntryCMPForSingleSupport.containsNaN()) computeEntryCMPForSupportFoot(cmp, supportSide, null, null); else
if (desiredCenterOfPressure.containsNaN() || centerOfPressure.containsNaN())
referenceICPToPack.scale(currentStateProjectionMultiplier.getPositionMultiplier()); if (!entryCMP.containsNaN()) if (!exitCMP.containsNaN()) if (!previousExitCMP.containsNaN()) if (!initialICP.containsNaN()) referenceICPVelocityToPack.scale(currentStateProjectionMultiplier.getVelocityMultiplier()); if (!entryCMP.containsNaN()) if (!exitCMP.containsNaN()) if (!previousExitCMP.containsNaN()) if (!initialICP.containsNaN())
numberOfValidCoPs += cop2ds.get(robotSide).containsNaN() ? 0 : 1; if (!cop2d.containsNaN())
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); footPolygon.clear(soleFrame); for (int i = 0; i < contactPoints.size(); i++) { contactPoints.get(i).getPosition2d(toeOffContactPoint2d); footPolygon.addVertex(toeOffContactPoint2d); } footPolygon.update(); FramePoint2d rayOrigin; if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d)) rayOrigin = exitCMP2d; else rayOrigin = footPolygon.getCentroid(); rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d); frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP); toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne()); contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame()); pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition); desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); desiredContactPointPosition.changeFrame(worldFrame); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredYawToHold = desiredOrientation.getYaw(); desiredRollToHold = desiredOrientation.getRoll(); }
if (desiredCMP.containsNaN())
momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); if (desiredCoP.containsNaN()) if (!cop.containsNaN()) if (cop.containsNaN())
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }
if (copDesired.containsNaN()) footSwitch.computeAndPackCoP(copActual); if (copActual.containsNaN())
kinematicExtreme); if (kinematicExtreme.containsNaN())
return; if (cop.containsNaN()) return;
if (!finalCapturePoint.containsNaN()) if (!finalCapturePoint.containsNaN())
if (cop2d.containsNaN()) cop2d.setToZero(contactableFoot.getSoleFrame()); framePosition.setXYIncludingFrame(cop2d);