private void drawPoint(FramePoint2d framePoint2d, Graphics graphics) { drawPoint(framePoint2d.getX(), framePoint2d.getY(), graphics); }
private void setReferenceFootstepLocation(int footstepIndex, FramePoint2d referenceFootstepLocation) { referenceFootstepLocation.changeFrame(worldFrame); referenceFootstepLocations.get(footstepIndex).set(0, 0, referenceFootstepLocation.getX()); referenceFootstepLocations.get(footstepIndex).set(1, 0, referenceFootstepLocation.getY()); }
public void resetFootstepRegularization(int footstepIndex, FramePoint2d previousFootstepLocation) { previousFootstepLocation.changeFrame(worldFrame); previousFootstepLocations.get(footstepIndex).set(0, 0, previousFootstepLocation.getX()); previousFootstepLocations.get(footstepIndex).set(1, 0, previousFootstepLocation.getY()); }
public ContactPoint(FramePoint2d point2d, PlaneContactState parentContactState) { position = new FramePoint(point2d.getReferenceFrame(), point2d.getX(), point2d.getY(), 0.0); this.parentContactState = parentContactState; }
public static void transformFramePoint2dIntoColumnVector(DenseMatrix64F matrix, FramePoint2d framePoint) { matrix.set(0, 0, framePoint.getX()); matrix.set(1, 0, framePoint.getY()); }
/** * Creates a new FramePoint based on the x and y components of this FramePoint2d */ public FramePoint toFramePoint() { return new FramePoint(this.getReferenceFrame(), this.getX(), this.getY(), 0.0); }
public double getZOnPlane(FramePoint2d xyPoint) { checkReferenceFrameMatch(xyPoint.getReferenceFrame()); return plane3d.getZOnPlane(xyPoint.getX(), xyPoint.getY()); }
public void update(FramePoint2d point2dUnfiltered) { checkReferenceFrameMatch(point2dUnfiltered); x.update(point2dUnfiltered.getX()); y.update(point2dUnfiltered.getY()); }
public SimpleFootstepMask(ReferenceFrame yawFrame2D, ContactablePlaneBody foot, double footKernelMaskSafetyBuffer) { this.yawFrame2d = yawFrame2D; this.safetyBuffer = footKernelMaskSafetyBuffer; ArrayList<FramePoint2d> contactPoints = new ArrayList<FramePoint2d>(); for (FramePoint2d point : foot.getContactPoints2d()) { contactPoints.add(new FramePoint2d(yawFrame2D, inflate(point.getX()), inflate(point.getY()))); } footPolygon = new FrameConvexPolygon2d(contactPoints); if (DEBUG) System.out.println("SimpleFootstepMask: footPolygon is \n" + footPolygon + " \nand yawFrame2d = \n" + yawFrame2d.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); }
private void drawInsidePoint(FramePoint2d testPoint, Graphics graphics) { int xInt = doubleToInt(testPoint.getX(), xCenter, scale, getWidth()); int yInt = doubleToInt(testPoint.getY(), yCenter, -scale, getHeight()); graphics.setColor(Color.green); graphics.fillOval(xInt - 2, yInt - 2, 4, 4); }
private void drawOutsidePoint(FramePoint2d testPoint, Graphics graphics) { int xInt = doubleToInt(testPoint.getX(), xCenter, scale, getWidth()); int yInt = doubleToInt(testPoint.getY(), yCenter, -scale, getHeight()); graphics.setColor(Color.red); graphics.fillOval(xInt - 2, yInt - 2, 4, 4); }
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); } }
public boolean findCenterOfClosestCell(FramePoint2d centerOfClosestCellToPack, FramePoint2d closestToPoint) { int xIndex = findXIndex(closestToPoint.getX()); int yIndex = findYIndex(closestToPoint.getY()); if (!checkIfIndicesAreValid(xIndex, yIndex)) return false; getCellCenter(centerOfClosestCellToPack, xIndex, yIndex); return true; }
private void findProjectionOntoPlaneFrame(ReferenceFrame planeFrame, FramePoint2d pointInWorld, FramePoint pointProjectedOntoPlaneFrameToPack) { pointInWorld.checkReferenceFrameMatch(worldFrame); double z = getPlaneZGivenXY(planeFrame, pointInWorld.getX(), pointInWorld.getY()); pointProjectedOntoPlaneFrameToPack.setXYIncludingFrame(pointInWorld); pointProjectedOntoPlaneFrameToPack.setZ(z); }
public static void computeWrench(Wrench groundReactionWrenchToPack, FrameVector force, FramePoint2d cop, double normalTorque) { ReferenceFrame referenceFrame = cop.getReferenceFrame(); force.changeFrame(referenceFrame); // r x f + tauN Vector3d torque = new Vector3d(); torque.setX(cop.getY() * force.getZ()); torque.setY(-cop.getX() * force.getZ()); torque.setZ(cop.getX() * force.getY() - cop.getY() * force.getX() + normalTorque); groundReactionWrenchToPack.set(referenceFrame, force.getVector(), torque); }
public static void computePseudoCMP3d(FramePoint pseudoCMP3dToPack, FramePoint centerOfMass, FramePoint2d cmp, double fZ, double totalMass, double omega0) { double zCMP = centerOfMass.getZ() - fZ / (totalMass * MathTools.square(omega0)); pseudoCMP3dToPack.setIncludingFrame(cmp.getReferenceFrame(), cmp.getX(), cmp.getY(), 0.0); pseudoCMP3dToPack.changeFrame(centerOfMass.getReferenceFrame()); pseudoCMP3dToPack.setZ(zCMP); }
/** * This function is called at beginning of every DoubleSupport, not frequent enough to preallocate eveything. */ public void updatePointAndPolygon(FrameConvexPolygon2d polygon, Point2d pointInPolygonFrame) { //copy external point back to polygon frame framePoint2d = new FramePoint2d(polygon.getReferenceFrame(), pointInPolygonFrame); //update polygon class point2dInConvexPolygon2d = new Point2dInConvexPolygon2d(polygon.getConvexPolygon2d(), framePoint2d.getX(), framePoint2d.getY()); angle.set(point2dInConvexPolygon2d.getAngle()); eccentricity.set(point2dInConvexPolygon2d.getEccentricity()); }
public void setDesiredCapturePointState(FramePoint2d currentDesiredCapturePointPosition, FrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
public boolean isCellAtLocationOccupied(FramePoint2d location) { location.checkReferenceFrameMatch(soleFrame); location.get(tempPoint); tempPoint.setX(tempPoint.getX() - gridOrigin.getX()); tempPoint.setY(tempPoint.getY() - gridOrigin.getY()); int xIndex = findXIndex(tempPoint.getX()); int yIndex = findYIndex(tempPoint.getY()); if (checkIfIndicesAreValid(xIndex, yIndex)) return occupancyGrid.get(xIndex, yIndex) > 0.9; else return false; }