public void setY(double y) { position.setY(y); }
public static void transformColumnVectorIntoFramePoint2d(DenseMatrix64F matrix, FramePoint2d framePoint) { framePoint.setX(matrix.get(0, 0)); framePoint.setY(matrix.get(1, 0)); }
public void getFootstepSolutionLocation(int footstepIndex, FramePoint2d footstepLocationToPack) { footstepLocationToPack.setToZero(worldFrame); footstepLocationToPack.setX(footstepLocationSolution.get(2 * footstepIndex, 0)); footstepLocationToPack.setY(footstepLocationSolution.get(2 * footstepIndex + 1, 0)); }
private void updatedFinalHeadingLine(double desiredHeadingFinal) { FramePoint2d endpoint1 = processedSensors.getCenterOfMassGroundProjectionInFrame(ReferenceFrame.getWorldFrame()).toFramePoint2d(); FramePoint2d endpoint2 = new FramePoint2d(endpoint1); double length = 1.0; endpoint2.setX(endpoint2.getX() + length * Math.cos(desiredHeadingFinal)); endpoint2.setY(endpoint2.getY() + length * Math.sin(desiredHeadingFinal)); FrameLineSegment2d frameLineSegment2d = new FrameLineSegment2d(endpoint1, endpoint2); finalHeadingLine.setFrameLineSegment2d(frameLineSegment2d); } }
private void updatedDesiredHeadingLine(double desiredHeading) { FramePoint2d endpoint1 = processedSensors.getCenterOfMassGroundProjectionInFrame(ReferenceFrame.getWorldFrame()).toFramePoint2d(); FramePoint2d endpoint2 = new FramePoint2d(endpoint1); double length = 1.0; endpoint2.setX(endpoint2.getX() + length * Math.cos(desiredHeading)); endpoint2.setY(endpoint2.getY() + length * Math.sin(desiredHeading)); FrameLineSegment2d frameLineSegment2d = new FrameLineSegment2d(endpoint1, endpoint2); desiredHeadingLine.setFrameLineSegment2d(frameLineSegment2d); }
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); }
@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); }