@Override public void getToeOffContactPoint(FramePoint2d contactPointToPack) { contactPointToPack.setIncludingFrame(toeOffContactPoint); } }
public ListOfPointsContactableFoot(RigidBody rigidBody, ReferenceFrame soleFrame, List<Point2d> contactPointsInSoleFrame, Point2d toeOffContactPoint) { super(rigidBody, soleFrame, contactPointsInSoleFrame); this.toeOffContactPoint.setIncludingFrame(soleFrame, toeOffContactPoint); }
public void setCapturePoint(FramePoint2d capturePoint2d) { this.capturePoint2d.setIncludingFrame(capturePoint2d); capturePointUpToDate = true; }
public void getFirstEndpoint(FramePoint2d firstEndpointToPack) { firstEndpointToPack.setIncludingFrame(referenceFrame, lineSegment.endpoints[0]); }
public void getSecondEndpoint(FramePoint2d secondEndpointToPack) { secondEndpointToPack.setIncludingFrame(referenceFrame, lineSegment.endpoints[1]); }
public void getCenterOfPressure(FramePoint2d centerOfPressureToPack, RigidBody foot) { centerOfPressureToPack.setIncludingFrame(soleFrames.get(foot), centerOfPressures.get(foot)); }
public void computeAndPackCoP(FramePoint2d copToPack) { updateCoP(); copToPack.setIncludingFrame(resolvedCoP); }
public void setPoseIncludingFrame(ReferenceFrame referenceFrame, double x, double y, double yaw) { this.referenceFrame = referenceFrame; position.setIncludingFrame(referenceFrame, x, y); orientation.setIncludingFrame(referenceFrame, yaw); }
public void getCellCenter(FramePoint2d cellCenter, int xIndex, int yIndex) { double x = getXCoordinateInSoleFrame(xIndex); double y = getYCoordinateInSoleFrame(yIndex); cellCenter.setIncludingFrame(soleFrame, x, y); }
public void updateForSingleSupport(FramePoint2d desiredCapturePoint2d, FramePoint2d capturePoint2d, double omega0) { if (!isEnabled()) return; this.omega0 = omega0; this.capturePoint2d.setIncludingFrame(capturePoint2d); this.desiredCapturePoint2d.setIncludingFrame(desiredCapturePoint2d); isICPErrorTooLarge.set(this.desiredCapturePoint2d.distance(this.capturePoint2d) > icpErrorThreshold.getDoubleValue()); }
public void getClosestVertex(FramePoint2d closestVertexToPack, FramePoint2d point) { point.checkReferenceFrameMatch(referenceFrame); closestVertexToPack.setIncludingFrame(referenceFrame, ConvexPolygon2dCalculator.getClosestVertexCopy(point.getPoint(), convexPolygon)); }
/** * Makes the pointToPack a FramePoint2d version of this FramePoint * * @param pointToPack */ public void getFramePoint2d(FramePoint2d pointToPack) { pointToPack.setIncludingFrame(this.getReferenceFrame(), this.getX(), this.getY()); }
/** * Add a vertex to this polygon after doing {@code newVertex.changeFrame(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). */ public void addVertexAndChangeFrame(FramePoint2d newVertex) { tempPoint2d.setIncludingFrame(newVertex); tempPoint2d.changeFrame(referenceFrame); convexPolygon.addVertex(tempPoint2d.getPoint()); }
/** * Add a vertex to this polygon after doing {@code newVertex.changeFrameAndProjectToXYPlane(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). */ public void addVertexChangeFrameAndProjectToXYPlane(FramePoint2d newVertex) { tempPoint2d.setIncludingFrame(newVertex); tempPoint2d.changeFrameAndProjectToXYPlane(referenceFrame); convexPolygon.addVertex(tempPoint2d.getPoint()); }
public void getClosestPointOnLineSegment(FramePoint2d framePoint2dToPack, FramePoint2d point) { checkReferenceFrameMatch(point); lineSegment.getClosestPointOnLineSegment(tempPoint2d, point.getPoint()); framePoint2dToPack.setIncludingFrame(referenceFrame, tempPoint2d); }
@Override public void getPosition2d(FramePoint2d framePoint2dToPack) { framePoint2dToPack.setIncludingFrame(getReferenceFrame(), position.getX(), position.getY()); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(point2d); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint2d.setIncludingFrame(currentReferenceFrame, point2d); framePoint2d.changeFrame(desiredFrame); framePoint2d.get(point2d); set(point2d); }
public void setIncludingFrame(FramePose2d pose) { this.referenceFrame = pose.getReferenceFrame(); position.setIncludingFrame(referenceFrame, pose.getX(), pose.getY()); orientation.setIncludingFrame(referenceFrame, pose.getYaw()); }
public void getDesiredCapturePointPositionAndVelocity(FramePoint2d desiredCapturePointPositionToPack, FrameVector2d desiredCapturePointVelocityToPack, FramePoint2d currentCapturePointPosition, double time) { super.getDesiredCapturePointPositionAndVelocity(desiredCapturePointPositionToPack, desiredCapturePointVelocityToPack, getTimeWithDelay(time)); if (doTimeFreezing.getBooleanValue()) { tmpCapturePointPosition.setIncludingFrame(desiredCapturePointPositionToPack); tmpCapturePointVelocity.setIncludingFrame(desiredCapturePointVelocityToPack); doTimeFreezeIfNeeded(currentCapturePointPosition, time); } previousTime.set(time); }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }