@Override public ReferenceFrame getReferenceFrame() { return point.getReferenceFrame(); }
public YoContactPoint(String namePrefix, int index, FramePoint2d contactPointPosition2d, PlaneContactState parentContactState, YoVariableRegistry parentRegistry) { this(namePrefix, index, contactPointPosition2d.getReferenceFrame(), parentContactState, parentRegistry); setPosition(contactPointPosition2d); }
public void setIncludingFrame(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this.referenceFrame = firstEndpoint.getReferenceFrame(); set(firstEndpoint, secondEndpoint); }
public FramePose2d(FramePoint2d position, FrameOrientation2d orientation) { if (position.getReferenceFrame() != orientation.getReferenceFrame()) { throw new ReferenceFrameMismatchException("FramePose: The position frame (" + position.getReferenceFrame() + ") does not match the orientation frame (" + orientation.getReferenceFrame() + ")"); } this.position = new FramePoint2d(position); this.orientation = new FrameOrientation2d(orientation); this.referenceFrame = position.getReferenceFrame(); }
public void set(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { referenceFrame.checkReferenceFrameMatch(firstEndpoint.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(secondEndpoint.getReferenceFrame()); frameLineSegment.set(firstEndpoint, secondEndpoint); getYoValuesFromFrameLineSegment(); }
public ContactPoint(FramePoint2d point2d, PlaneContactState parentContactState) { position = new FramePoint(point2d.getReferenceFrame(), point2d.getX(), point2d.getY(), 0.0); this.parentContactState = parentContactState; }
/** * 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 FrameLine2d(FramePoint2d framePoint2d, FrameVector2d frameVector2d) { this(framePoint2d.getReferenceFrame(), new Line2d(framePoint2d.getPointCopy(), frameVector2d.getVectorCopy())); framePoint2d.checkReferenceFrameMatch(frameVector2d); }
public void setIncludingFrame(FramePoint2d startPoint, FrameVector2d vector) { startPoint.checkReferenceFrameMatch(vector); this.referenceFrame = startPoint.getReferenceFrame(); this.line.set(startPoint.getPoint(), vector.getVector()); }
@Override public FramePoint2d orthogonalProjectionCopy(FramePoint2d point) { checkReferenceFrameMatch(point); Point2d projected = convexPolygon.orthogonalProjectionCopy(point.getPoint()); if (projected == null) { return null; } return new FramePoint2d(point.getReferenceFrame(), projected); }
public double getZOnPlane(FramePoint2d xyPoint) { checkReferenceFrameMatch(xyPoint.getReferenceFrame()); return plane3d.getZOnPlane(xyPoint.getX(), xyPoint.getY()); }
@Override public FramePoint2d orthogonalProjectionCopy(FramePoint2d point) { checkReferenceFrameMatch(point); Point2d projected = line.orthogonalProjectionCopy(point.getPoint()); return new FramePoint2d(point.getReferenceFrame(), projected); }
public FrameLineSegment2d(FramePoint2d[] endpoints) { this(endpoints[0].getReferenceFrame(), new LineSegment2d(endpoints[0].getPointCopy(), endpoints[1].getPointCopy())); endpoints[0].checkReferenceFrameMatch(endpoints[1]); }
@Override public FramePoint2d orthogonalProjectionCopy(FramePoint2d point) { checkReferenceFrameMatch(point); Point2d projected = lineSegment.orthogonalProjectionCopy(point.getPoint()); return new FramePoint2d(point.getReferenceFrame(), projected); }
public FrameLineSegment2d(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this(firstEndpoint.getReferenceFrame(), new LineSegment2d(firstEndpoint.getPointCopy(), secondEndpoint.getPointCopy())); firstEndpoint.checkReferenceFrameMatch(secondEndpoint); }
public FrameLine2d(FramePoint2d firstPointOnLine, FramePoint2d secondPointOnLine) { this(firstPointOnLine.getReferenceFrame(), new Line2d(firstPointOnLine.getPointCopy(), secondPointOnLine.getPointCopy())); firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
public void set(FramePoint2d firstEndpoint, FrameVector2d vectorToSecondEndpoint) { referenceFrame.checkReferenceFrameMatch(firstEndpoint.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(vectorToSecondEndpoint.getReferenceFrame()); frameLineSegment.set(firstEndpoint, vectorToSecondEndpoint); getYoValuesFromFrameLineSegment(); }
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); }
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()); }