How to use

getCentroid

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

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);