public void getPosition(FixedFramePoint3DBasics positionToPack) { positionToPack.set(position); }
/** * Sets this point coordinate to the given {@code referenceFrame}'s origin coordinate in this frame * tuple current frame. * * @param referenceFrame the reference frame of interest. */ default void setFromReferenceFrame(ReferenceFrame referenceFrame) { setToZero(); referenceFrame.transformFromThisToDesiredFrame(getReferenceFrame(), this); } }
/** * Computes the orthogonal projection of a 3D point on a given 3D plane defined by a 3D point and 3D * normal. * <p> * Edge cases: * <ul> * <li>if the length of the plane normal is too small, i.e. less than * {@link EuclidGeometryTools#ONE_TRILLIONTH}, this method fails and returns {@code false}. * </ul> * </p> * * @param x the x-coordinate of the point to compute the projection of. Not modified. * @param y the y-coordinate of the point to compute the projection of. Not modified. * @param z the z-coordinate of the point to compute the projection of. Not modified. * @param pointOnPlane a point on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @param projectionToPack point in which the projection of the point onto the plane is stored. * Modified. * @return whether the method succeeded or not. * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, FixedFramePoint3DBasics projectionToPack) { pointOnPlane.checkReferenceFrameMatch(planeNormal); projectionToPack.checkReferenceFrameMatch(pointOnPlane); return EuclidGeometryTools.orthogonalProjectionOnPlane3D(x, y, z, pointOnPlane, planeNormal, projectionToPack); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2D vectorToPack, FramePose3D fromPose, FramePose3D toPose) { if (fromPose.getPosition().epsilonEquals(toPose.getPosition(), 1e-7)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2D frameTuple2d = new FrameVector2D(vectorToPack); frameTuple2d.set(toPose.getPosition()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.set((Tuple2DReadOnly) frameTuple2d); } }
public ConvexPolygon2D snapAndWiggle(FramePose3D solePose, ConvexPolygon2DReadOnly footStepPolygon, boolean walkingForward) throws SnappingFailedException { if (planarRegionsList == null) { return null; } planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getConvexHull().getArea() < parameters.getMinPlanarRegionArea()); planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getNormal().getZ() < Math.cos(parameters.getMaxPlanarRegionAngle())); FramePose3D solePoseBeforeSnapping = new FramePose3D(solePose); PoseReferenceFrame soleFrameBeforeSnapping = new PoseReferenceFrame("SoleFrameBeforeSnapping", solePose); FrameConvexPolygon2D footPolygon = new FrameConvexPolygon2D(soleFrameBeforeSnapping, footStepPolygon); footPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); // this works if the soleFrames are z up. if(isOnBoundaryOfPlanarRegions(planarRegionsList, footPolygon)) { /* * If foot is on the boundary of planar regions, don't snap/wiggle but * set it to the nearest plane's height */ FixedFramePoint3DBasics footPosition = solePose.getPosition(); PlanarRegion closestRegion = planarRegionsList.findClosestPlanarRegionToPointByProjectionOntoXYPlane(footPosition.getX(), footPosition.getY()); solePose.setZ(closestRegion.getPlaneZGivenXY(footPosition.getX(), footPosition.getY())); return new ConvexPolygon2D(footStepPolygon); } ConvexPolygon2D foothold = doSnapAndWiggle(solePose, footStepPolygon, footPolygon); checkAndHandleTopOfCliff(solePoseBeforeSnapping, solePose, walkingForward, footStepPolygon, footPolygon); checkAndHandleBottomOfCliff(solePose); return foothold; }
private boolean checkFarEnoughFromStart(FootstepNode rejectedNode) { tempPoint2D.set(rejectedNode.getX(), rejectedNode.getY()); return initialPose.getPosition().distanceXY(tempPoint2D) > distanceFromStartToConsiderSearching; }
pointOnLine1.checkReferenceFrameMatch(lineDirection2); if (closestPointOnLine1ToPack != null) closestPointOnLine1ToPack.checkReferenceFrameMatch(pointOnLine1); if (closestPointOnLine2ToPack != null) closestPointOnLine2ToPack.checkReferenceFrameMatch(pointOnLine1); return EuclidGeometryTools.closestPoint3DsBetweenTwoLine3Ds(pointOnLine1, lineDirection1, pointOnLine2, lineDirection2, closestPointOnLine1ToPack, closestPointOnLine2ToPack);
footstep.getSoleFramePose(pose); pose.changeFrame(ReferenceFrame.getWorldFrame()); assertTrue(pose.getPosition().epsilonEquals(new Point3D(1.0, 0.0, 0.0), 0.15));
private boolean checkFarEnoughFromEnd(FootstepNode rejectedNode) { tempPoint2D.set(rejectedNode.getX(), rejectedNode.getY()); return finalPose.getPosition().distanceXY(tempPoint2D) > distanceFromGoalToConsiderSearching; }
/** * Changes the first endpoint of this line segment. * * @param firstEndpoint new endpoint of this line segment. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code firstEndpoint} are not * expressed in the same reference frame. */ default void setFirstEndpoint(FramePoint3DReadOnly firstEndpoint) { getFirstEndpoint().set(firstEndpoint); }
lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd2); if (closestPointOnLineSegment1ToPack != null) closestPointOnLineSegment1ToPack.checkReferenceFrameMatch(lineSegmentStart1); if (closestPointOnLineSegment2ToPack != null) closestPointOnLineSegment2ToPack.checkReferenceFrameMatch(lineSegmentStart1); return EuclidGeometryTools.closestPoint3DsBetweenTwoLineSegment3Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, closestPointOnLineSegment1ToPack, closestPointOnLineSegment2ToPack);
/** * Changes the second endpoint of this line segment. * * @param secondEndpoint new second endpoint of this line segment. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code secondEndpoint} are not * expressed in the same reference frame. */ default void setSecondEndpoint(FramePoint3DReadOnly secondEndpoint) { getSecondEndpoint().set(secondEndpoint); }
/** * Assuming an isosceles triangle defined by three vertices A, B, and C, with |AB| == |BC|, this * methods computes the missing vertex B given the vertices A and C, the normal of the triangle, the * angle ABC that is equal to the angle at B from the the leg BA to the leg BC. * <a href="https://en.wikipedia.org/wiki/Isosceles_triangle"> Useful link</a>. * * @param baseVertexA the first base vertex of the isosceles triangle ABC. Not modified. * @param baseVertexC the second base vertex of the isosceles triangle ABC. Not modified. * @param trianglePlaneNormal the normal of the plane on which is lying. Not modified. * @param ccwAngleAboutNormalAtTopVertex the angle at B from the the leg BA to the leg BC. * @param topVertexBToPack the missing vertex B. Modified. * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVertexA, FramePoint3DReadOnly baseVertexC, FrameVector3DReadOnly trianglePlaneNormal, double ccwAngleAboutNormalAtTopVertex, FixedFramePoint3DBasics topVertexBToPack) { baseVertexA.checkReferenceFrameMatch(baseVertexC); baseVertexA.checkReferenceFrameMatch(trianglePlaneNormal); topVertexBToPack.checkReferenceFrameMatch(baseVertexA); EuclidGeometryTools.topVertex3DOfIsoscelesTriangle3D(baseVertexA, baseVertexC, trianglePlaneNormal, ccwAngleAboutNormalAtTopVertex, topVertexBToPack); }
private FramePose3DReadOnly adjustFootstep(FramePose2DReadOnly footstepPose, RobotSide footSide) adjustedBasedOnStanceFoot.getPosition().set(footstepPose.getPosition()); adjustedBasedOnStanceFoot.setZ(continuousStepGenerator.getCurrentSupportFootPose().getZ()); adjustedBasedOnStanceFoot.setOrientation(footstepPose.getOrientation());
/** * Computes the orthogonal projection of a 3D point on a given 3D plane defined by a 3D point and 3D * normal. * <p> * Edge cases: * <ul> * <li>if the length of the plane normal is too small, i.e. less than * {@link EuclidGeometryTools#ONE_TRILLIONTH}, this method fails and returns {@code false}. * </ul> * </p> * * @param pointToProject the point to compute the projection of. Not modified. * @param pointOnPlane a point on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @param projectionToPack point in which the projection of the point onto the plane is stored. * Modified. * @return whether the method succeeded or not. * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, FixedFramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnPlane); pointToProject.checkReferenceFrameMatch(planeNormal); projectionToPack.checkReferenceFrameMatch(pointToProject); return EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal, projectionToPack); }
/** * Computes the orthogonal projection of a 3D point on an infinitely long 3D line defined by a 3D * point and a 3D direction. * <p> * Edge cases: * <ul> * <li>if the given line direction is too small, i.e. * {@code lineDirection.lengthSquared() < }{@link EuclidGeometryTools#ONE_TRILLIONTH}, this method * fails and returns {@code false}. * </ul> * </p> * * @param pointToProject the point to compute the projection of. Not modified. * @param pointOnLine point located on the line. Not modified. * @param lineDirection direction of the line. Not modified. * @param projectionToPack point in which the projection of the point onto the line is stored. * Modified. * @return whether the method succeeded or not. * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnLine); pointToProject.checkReferenceFrameMatch(lineDirection); projectionToPack.checkReferenceFrameMatch(pointToProject); return EuclidGeometryTools.orthogonalProjectionOnLine3D(pointToProject, pointOnLine, lineDirection, projectionToPack); }
projectionToPack.checkReferenceFrameMatch(pointToProject); return EuclidGeometryTools.orthogonalProjectionOnLineSegment3D(pointToProject, lineSegmentStart, lineSegmentEnd, projectionToPack);
orthogonalProjectionToPack.checkReferenceFrameMatch(point); perpendicularVectorToPack.checkReferenceFrameMatch(point); return EuclidGeometryTools.perpendicularVector3DFromLine3DToPoint3D(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack,
orthogonalProjectionToPack.checkReferenceFrameMatch(point); Vector3D perpendicularVector = EuclidGeometryTools.perpendicularVector3DFromLine3DToPoint3D(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack);