s0.setX(xS0); d0.setX(xD0); dF.setX(xDF); sF.setX(xSF); sNext.setX(xSNext); s0Min.setX(xS0); d0Min.setX(xD0); dFMin.setX(xDF); sFMin.setX(xSF); sNextMin.setX(xSNext); s0Nom.setX(xS0); d0Nom.setX(xD0); dFNom.setX(xDF); sFNom.setX(xSF); sNextNom.setX(xSNext); s0Max.setX(xS0); d0Max.setX(xD0); dFMax.setX(xDF); sFMax.setX(xSF); sNextMax.setX(xSNext);
public CubicSplineCurveGenerator(double[] points) { Point2d[] arrayOfPoints = new Point2d[points.length]; double numberOfPoints = points.length; for (int i = 0; i < points.length; i++) { arrayOfPoints[i] = new Point2d(); arrayOfPoints[i].setY(points[i]); arrayOfPoints[i].setX((i + 1.0) / numberOfPoints); } init(arrayOfPoints); }
public static void getLowerHull(ArrayList<Point2d> lowerHullToPack, ArrayList<Point2d> pointList) { /* * Changes the points in pointList, but changes them back after the hull was found. * The reason for this is that the object reference remains the same. * Only the sign is changed, so there are no round-off errors. */ lowerHullToPack.clear(); for(int i = 0; i < pointList.size(); i++) { Point2d point = pointList.get(i); point.setX(-point.getX()); point.setY(-point.getY()); } getUpperHull(lowerHullToPack, pointList); for(int i = 0; i < pointList.size(); i++) { Point2d point = pointList.get(i); point.setX(-point.getX()); point.setY(-point.getY()); } }
public void getPoint2d(Point2d pointToPack) { pointToPack.setX(this.getX()); pointToPack.setY(this.getY()); }
private Point2d getOffsetPosition(FramePose2d currentPose, RobotSide currentSide, double horizontalDistance, double footYaw) { Point2d position = new Point2d(currentPose.getX(), currentPose.getY()); double yaw = footYaw; double sign = (currentSide == RobotSide.LEFT) ? 1 : -1; // sign * horizontalDistance position.setX(position.getX() + -1.0 * Math.sin(yaw) * sign * horizontalDistance); position.setY(position.getY() + 1.0 * Math.cos(yaw) * sign * horizontalDistance); return position; }
public void getMidpoint(Point2d midpointToPack) { midpointToPack.setX((endpoints[0].getX() + endpoints[1].getX()) / 2.0); midpointToPack.setY((endpoints[0].getY() + endpoints[1].getY()) / 2.0); }
public static SideDependentList<ConvexPolygon2d> createDefaultFootPolygons(RobotContactPointParameters contactPointParameters, double scalingFactorForFootholdX, double scalingFactorForFootholdY) { SideDependentList<ConvexPolygon2d> footPolygons = new SideDependentList<>(); for (RobotSide side : RobotSide.values) { ArrayList<Point2d> footPoints = contactPointParameters.getFootContactPoints().get(side); ArrayList<Point2d> scaledFootPoints = new ArrayList<Point2d>(); for(int i = 0; i < footPoints.size(); i++) { Point2d footPoint = new Point2d(footPoints.get(i)); footPoint.setX(footPoint.getX() * scalingFactorForFootholdX); footPoint.setY(footPoint.getY() * scalingFactorForFootholdY); scaledFootPoints.add(footPoint); } ConvexPolygon2d scaledFoot = new ConvexPolygon2d(scaledFootPoints); footPolygons.set(side, scaledFoot); } return footPolygons; }
projectionToPack.setX(pointOnLineX + alpha * lineDirectionX); projectionToPack.setY(pointOnLineY + alpha * lineDirectionY);
public void getPointGivenParameter(double t, Point2d pointToPack) { pointToPack.set(point); pointToPack.setX(pointToPack.getX() + t * normalizedVector.getX()); pointToPack.setY(pointToPack.getY() + t * normalizedVector.getY()); }
public void getPointGivenParameters(Point2d point, double xParameter, double yParameter) { point.setX(minPoint.getX() + xParameter * (maxPoint.getX() - minPoint.getX())); point.setY(minPoint.getY() + yParameter * (maxPoint.getY() - minPoint.getY())); }
double midPointY = 0.5 * (lineSegmentStart.getY() + lineSegmentEnd.getY()); bisectorSegmentStartToPack.setX(midPointX + bisectorDirectionX * bisectorSegmentHalfLength); bisectorSegmentStartToPack.setY(midPointY + bisectorDirectionY * bisectorSegmentHalfLength); bisectorSegmentEndToPack.setX(midPointX - bisectorDirectionX * bisectorSegmentHalfLength); bisectorSegmentEndToPack.setY(midPointY - bisectorDirectionY * bisectorSegmentHalfLength);
projectionToPack.setX((1.0 - percentage) * lineSegmentStartX + percentage * lineSegmentEndX); projectionToPack.setY((1.0 - percentage) * lineSegmentStartY + percentage * lineSegmentEndY); return true;
@Override public void applyTransformAndProjectToXYPlane(RigidBodyTransform transform) { isUpToDate = false; for (int i = 0; i < numberOfVertices; i++) { Point2d vertex = getVertexUnsafe(i); tempVertex3d.set(vertex.getX(), vertex.getY(), 0.0); transform.transform(tempVertex3d); vertex.setX(tempVertex3d.getX()); vertex.setY(tempVertex3d.getY()); } update(); }
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; }
public static FramePoint getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter) { Point2d footstepCenter; List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null) { ConvexPolygon2d contactPolygon = new ConvexPolygon2d(predictedContactPoints); footstepCenter = contactPolygon.getCentroid(); } else { footstepCenter = new Point2d(); } footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter); footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter)); FramePoint footstepCenter3d = new FramePoint(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0); footstepCenter3d.changeFrame(worldFrame); return footstepCenter3d; }
sNext.setX(sF.getX() + 0.01);
if (crossProduct <= 0.0) referencePointInPCopy.setX(referencePointInP.getX() + vertexQ.getX() - vertexP.getX()); referencePointInPCopy.setY(referencePointInP.getY() + vertexQ.getY() - vertexP.getY()); Line2d ray = new Line2d(referencePointInPCopy, edgeOnQ);
public void registerCenterOfPressureLocation(FramePoint2d copToRegister) { copToRegister.checkReferenceFrameMatch(soleFrame); copToRegister.get(tempPoint); tempPoint.setX(tempPoint.getX() - gridOrigin.getX()); tempPoint.setY(tempPoint.getY() - gridOrigin.getY()); int xIndex = findXIndex(tempPoint.getX()); int yIndex = findYIndex(tempPoint.getY()); currentXIndex.set(xIndex); currentYIndex.set(yIndex); areCurrentCoPIndicesValid.set(checkIfIndicesAreValid(xIndex, yIndex)); if (areCurrentCoPIndicesValid.getBooleanValue()) { counterGrid.add(xIndex, yIndex, 1.0); if (counterGrid.get(xIndex, yIndex) >= thresholdForCellActivation.getDoubleValue()) { occupancyGrid.set(xIndex, yIndex, 1.0); if (VISUALIZE) { getCellCenter(cellCenter, xIndex, yIndex); cellPosition.setXYIncludingFrame(cellCenter); cellViz[xIndex][yIndex].setAndMatchFrame(cellPosition); } } else { occupancyGrid.set(xIndex, yIndex, 0.0); } } }
public void setGoalPose(FramePose goalPose) { goalPose.getPosition2dIncludingFrame(this.goalPosition); Tuple3d goalPosition = new Point3d(); goalPose.getPosition(goalPosition); FootstepPlannerGoal footstepPlannerGoal = new FootstepPlannerGoal(); footstepPlannerGoal.setGoalPoseBetweenFeet(goalPose); Point2d xyGoal = new Point2d(); xyGoal.setX(goalPose.getX()); xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal); footstepPlannerGoal.setFootstepPlannerGoalType(FootstepPlannerGoalType.CLOSE_TO_XY_POSITION); FramePose leftFootPose = new FramePose(); leftFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.LEFT)); leftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); sendPacketToUI(new UIPositionCheckerPacket(new Point3d(xyGoal.getX(), xyGoal.getY(), leftFootPose.getZ()), new Quat4d())); footstepPlanner.setGoal(footstepPlannerGoal); footstepPlanner.setInitialStanceFoot(leftFootPose, RobotSide.LEFT); }