public FramePoint2d getCentroid() { getCentroid(temporaryCentroid); return temporaryCentroid; }
public void getShrunkPolygonCentroid(FramePoint2d centroidToPack) { shrunkFootPolygon.getCentroid(centroidToPack); }
public FramePoint2d getCentroidCopy() { FramePoint2d centroidToReturn = new FramePoint2d(); getCentroid(centroidToReturn); return centroidToReturn; }
public void setContactFramePoints(List<FramePoint2d> contactPointLocations) { int contactPointLocationsSize = contactPointLocations.size(); if (contactPointLocationsSize != totalNumberOfContactPoints) throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints"); for (int i = 0; i < contactPointLocationsSize; i++) { FramePoint2d contactPointLocation = contactPointLocations.get(i); YoContactPoint yoContactPoint = contactPoints.get(i); yoContactPoint.setPosition(contactPointLocation); } contactPointsPolygon.setIncludingFrameAndUpdate(contactPointLocations); this.contactPointCentroid.set(contactPointsPolygon.getCentroid()); }
public void setContactPoints(List<Point2d> contactPointLocations) { int contactPointLocationsSize = contactPointLocations.size(); if (contactPointLocationsSize != totalNumberOfContactPoints) throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints"); for (int i = 0; i < contactPointLocationsSize; i++) { Point2d contactPointLocation = contactPointLocations.get(i); YoContactPoint yoContactPoint = contactPoints.get(i); yoContactPoint.setPosition2d(contactPointLocation); } contactPointsPolygon.setIncludingFrameAndUpdate(planeFrame, contactPointLocations); this.contactPointCentroid.set(contactPointsPolygon.getCentroid()); }
private void computeFootstepCentroid(FramePoint2d centroidToPack, Footstep footstep) { predictedSupportPolygon.clear(footstep.getSoleReferenceFrame()); addPredictedContactPointsToPolygon(footstep, predictedSupportPolygon); predictedSupportPolygon.update(); predictedSupportPolygon.getCentroid(centroidToPack); }
private void computePredictedSupportCentroid(FramePoint2d centroidToPack, Footstep footstep, Footstep nextFootstep) { predictedSupportPolygon.clear(worldFrame); addPredictedContactPointsToPolygon(footstep, predictedSupportPolygon); addPredictedContactPointsToPolygon(nextFootstep, predictedSupportPolygon); predictedSupportPolygon.update(); predictedSupportPolygon.getCentroid(centroidToPack); }
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
private void computeReferenceCMPsWithUpcomingFootsteps(RobotSide firstSupportSide, int numberOfUpcomingFootsteps, int cmpIndex) FramePoint2d centroidInSoleFrameOfPreviousSupportFoot = supportFootPolygonsInSoleZUpFrame.get(firstSupportSide).getCentroid();
private void projectCMPIntoSupportPolygonIfOutsideLocal(FrameConvexPolygon2d supportPolygon, FramePoint2d desiredCMPToPack) { cmpProjected.set(false); if (supportPolygon.getArea() < 1.0e-3) { supportPolygon.getCentroid(desiredCMPToPack); return; } if (supportPolygon.isPointInside(projectedCMP)) { return; } else { supportPolygon.getClosestVertex(projectedCMP, projectedCMP); cmpProjected.set(true); } desiredCMPToPack.setX(projectedCMP.getX()); }
private void putExitCMPOnToes(FrameConvexPolygon2d footSupportPolygon, FramePoint2d exitCMPToPack) { // Set x to have the CMP slightly inside the support polygon exitCMPToPack.setToZero(footSupportPolygon.getReferenceFrame()); exitCMPToPack.setX(footSupportPolygon.getMaxX() - 1.6e-2); exitCMPToPack.setY(footSupportPolygon.getCentroid().getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdgesWhenSteppingDown.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(exitCMPToPack); }
private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2d cmpToPack, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset) { // First constrain the computed CMP to the given min/max along the x-axis. FramePoint2d footSupportCentroid = footSupportPolygon.getCentroid(); double cmpXOffsetFromCentroid = stepLengthToCMPOffsetFactor.getDoubleValue() * (centroidOfFootstepToConsider.getX() - footSupportCentroid.getX()) + cmpOffset.getX(); cmpXOffsetFromCentroid = MathTools.clipToMinMax(cmpXOffsetFromCentroid, minForwardCMPOffset, maxForwardCMPOffset); cmpToPack.setIncludingFrame(footSupportCentroid); cmpToPack.add(cmpXOffsetFromCentroid, cmpOffset.getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(cmpToPack); }
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
@Override public void projectCMPIntoSupportPolygonIfOutside(FramePoint2d capturePoint, FrameConvexPolygon2d supportPolygon, FramePoint2d finalDesiredCapturePoint, FramePoint2d desiredCMPToPack) { ReferenceFrame returnFrame = desiredCMPToPack.getReferenceFrame(); desiredCMPToPack.changeFrame(supportPolygon.getReferenceFrame()); capturePoint.changeFrame(supportPolygon.getReferenceFrame()); projectedCMP.setToZero(supportPolygon.getReferenceFrame()); projectedCMP.setX(desiredCMPToPack.getX()); projectedCMP.setY(supportPolygon.getCentroid().getY()); projectCMPIntoSupportPolygonIfOutsideLocal(supportPolygon, desiredCMPToPack); desiredCMPToPack.changeFrame(returnFrame); capturePoint.changeFrame(returnFrame); }
@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(); }
public void checkIfRobotIsFalling(FramePoint2d capturePoint2d, FramePoint2d desiredCapturePoint2d) { updateCombinedPolygon(); if (isUsingNextFootstep.getBooleanValue()) icpDistanceFromFootPolygon.set(combinedFootPolygonWithNextFootstep.distance(capturePoint2d)); else icpDistanceFromFootPolygon.set(combinedFootPolygon.distance(capturePoint2d)); // TODO need to investigate this method, seems to be buggy // boolean isCapturePointCloseToFootPolygon = combinedFootPolygon.isPointInside(capturePoint, icpDistanceFromFootPolygonThreshold.getDoubleValue()); boolean isCapturePointCloseToFootPolygon = icpDistanceFromFootPolygon.getDoubleValue() < icpDistanceFromFootPolygonThreshold.getDoubleValue(); boolean isCapturePointCloseToDesiredCapturePoint = desiredCapturePoint2d.distance(capturePoint2d) < icpDistanceFromFootPolygonThreshold.getDoubleValue(); isRobotFalling.set(!isCapturePointCloseToFootPolygon && !isCapturePointCloseToDesiredCapturePoint); if (isRobotFalling.getBooleanValue()) { tempFallingDirection.set(capturePoint2d); FramePoint2d footCenter = combinedFootPolygon.getCentroid(); tempFallingDirection.changeFrame(ReferenceFrame.getWorldFrame()); footCenter.changeFrame(ReferenceFrame.getWorldFrame()); tempFallingDirection.sub(footCenter); fallingDirection.set(tempFallingDirection); } }
FramePoint2d centroid = supportPolygon.getCentroid();
/** * This function projects the footstep midpoint in the capture region. * Might be a bit conservative it should be sufficient to slightly overlap the capture region * and the touch-down polygon. */ private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion) { projection.setIncludingFrame(projectionPoint); projection.changeFrame(footstep.getParentFrame()); // move the position of the footstep to the capture region centroid nextStep2d.setIncludingFrame(captureRegion.getCentroid()); nextStep2d.changeFrame(footstep.getParentFrame()); // move the position as far away from the projectionPoint as possible direction.setIncludingFrame(nextStep2d); direction.sub(projection); direction.normalize(); direction.scale(10.0); nextStep2d.add(direction); nextStep2d.changeFrame(captureRegion.getReferenceFrame()); captureRegion.orthogonalProjection(nextStep2d); nextStep2d.changeFrame(footstep.getParentFrame()); footstep.setPositionChangeOnlyXY(nextStep2d); calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon); }
double rotationOnEdge = edgeVector.dot(desiredRotationVector); if (closestEdgeToCoP.isPointOnLeftSideOfLineSegment(footPolygon.getCentroid()))
projectedCapturePoint2d.setByProjectionOntoXYPlaneIncludingFrame(projectedCapturePoint); distanceICPToFeet.get(robotSide).set(projectedCapturePoint2d.distance(footPolygon.getCentroid())); isRobotBackToSafeState.set(false);