/** * Returns the value of the y-coordinate of this point. * * @return the y-coordinate's value. */ @Override public double getY() { return point.getY(); }
public BoxInfo(Point3D center, double[] size) { this.center = center; centerX = center.getX(); centerY = center.getY(); sizeX = size[0]; sizeY = size[1]; sizeZ = size[2]; } }
public static float[] toPointCloudFloatArrayInYUp(List<Point3D> points) { float[] ret = new float[points.size() * 3]; for (int i = 0; i < points.size(); i++) { Point3D point3d = points.get(i); ret[i * 3 + 0] = (float) point3d.getY(); ret[i * 3 + 1] = (float) point3d.getZ(); ret[i * 3 + 2] = (float) point3d.getX(); } return ret; }
private static void doBrakeDown3D(ArrayList<Point3D> points, int index, Point3D point1, Point3D point2) { if (point2.distance(point1) > 0.2) { Point3D newPointToAdd = new Point3D(point1.getX() + ((point2.getX() - point1.getX()) / 2.0), point1.getY() + ((point2.getY() - point1.getY()) / 2.0), point1.getZ() + ((point2.getZ() - point1.getZ()) / 2.0)); points.add(index, new Point3D(newPointToAdd.getX(), newPointToAdd.getY(), 0)); doBrakeDown3D(points, index, point1, new Point3D(newPointToAdd.getX(), newPointToAdd.getY(), 0)); } }
private void assertPointEquals(Point3D expectedPoint, Point3D actualPoint) { String failMessage = "Expected <(" + expectedPoint.getX() + "," + expectedPoint.getY() + "," + expectedPoint.getZ() + ")>, but was <(" + actualPoint.getX() + "," + actualPoint.getY() + "," + actualPoint.getZ() + ")>"; assertEquals(failMessage, expectedPoint.getX(), actualPoint.getX(), EPSILON); assertEquals(failMessage, expectedPoint.getY(), actualPoint.getY(), EPSILON); assertEquals(failMessage, expectedPoint.getZ(), actualPoint.getZ(), EPSILON); }
private QuadTreeTestHelper testOnASlope(Point3D center, Vector3D normal, double halfWidth, double resolution, boolean visualize) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForSlope(plane3d, halfWidth, resolution); int pointsPerBallUpdate = 1; return testOnAListOfPoints(points, pointsPerBallUpdate, boundingBox, resolution, visualize); }
private void testOnAStaircase(Point3D center, Vector3D normal, double halfWidth, double resolution, double stairSeparation, double oneStairLandingHeight) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForStairs(plane3d, halfWidth, resolution, stairSeparation, oneStairLandingHeight); // Collections.shuffle(points); testOnAListOfPoints(points, boundingBox, resolution); }
private void addRandomPointsToQuadTree(Random random, int numberOfPoints, Box bounds, double minZ, double maxZ, QuadTreeForGround quadTree) { for (int i = 0; i < numberOfPoints; i++) { Point3D point = RandomGeometry.nextPoint3D(random, bounds.minX, bounds.minY, minZ, bounds.maxX, bounds.maxY, maxZ); quadTree.put(point.getX(), point.getY(), point.getZ()); } }
public void assertPointsLieOnHeightMap(ArrayList<Point3D> points) { if (DO_ASSERTS) { for (Point3D point : points) { double heightMapZ = heightMap.getHeightAtPoint(point.getX(), point.getY()); assertEquals(point.getZ(), heightMapZ, 1e-7); } } }
private Vector3D generateRandomVector3d(Random random, Point3D minValues, Point3D maxValues) { double x = generateRandomDoubleBetween(random, minValues.getX(), maxValues.getX()); double y = generateRandomDoubleBetween(random, minValues.getY(), maxValues.getY()); double z = generateRandomDoubleBetween(random, minValues.getZ(), maxValues.getZ()); return new Vector3D(x, y, z); }
@Override public boolean currentlyMeetsGoal() { return Math.abs(variables.getRobotBodyX().getDoubleValue() - point.position_.getX()) < positionDelta && Math.abs(variables.getRobotBodyY().getDoubleValue() - point.position_.getY()) < positionDelta && Math.abs(variables.getRobotBodyYaw().getDoubleValue() - point.position_.getZ()) < yawDelta && Math.abs((variables.getYoTime().getDoubleValue() - initialTime) - point.getTime()) < timeDelta; }
private void setGoalPosition(Point3D position) { goalSphere.setTranslateX(position.getX()); goalSphere.setTranslateY(position.getY()); goalSphere.setTranslateZ(position.getZ()); }
public void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.getX(), scsCameraPosition.getY(), scsCameraPosition.getZ()); scs.setCameraFix(scsCameraFix.getX(), scsCameraFix.getY(), scsCameraFix.getZ()); scs.startOnAThread(); }
private void testHeightAtRampForAnyRampWithTranslation(Point3D[] pointsOnRamp, RotatableRampTerrainObject ramp, Vector3D translation) { for (int i = 0; i < pointsOnRamp.length; i++) { String message = "Expected Height For point " + (pointsOnRamp[i].getX()+translation.getX()) + " " + (pointsOnRamp[i].getY()+translation.getY()) + " " + pointsOnRamp[i].getZ(); assertEquals(message, pointsOnRamp[i].getZ(), ramp.heightAt(pointsOnRamp[i].getX()+translation.getX(), pointsOnRamp[i].getY()+translation.getY(), pointsOnRamp[i].getZ()), epsilon); } }
@Override protected void setBehaviorInput() { publishTextToSpeack("Picking Up The Ball"); FramePoint3D point = new FramePoint3D(ReferenceFrame.getWorldFrame(), grabLocation.getX(), grabLocation.getY(), grabLocation.getZ() + objectRadius + 0.25); atlasPrimitiveActions.wholeBodyBehavior.setSolutionQualityThreshold(2.01); atlasPrimitiveActions.wholeBodyBehavior.setTrajectoryTime(3); FrameQuaternion tmpOr = new FrameQuaternion(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); atlasPrimitiveActions.wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
private double getAngleToPelvis(Point3D point, Point3D lidarOrigin) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3D tfPoint = new Point3D(point); tf.transform(tfPoint); return Math.atan2(tfPoint.getY(), tfPoint.getX()); }
protected boolean isValidNearScan(Point3D point, Point3D lidarOrigin) { boolean valid = true; valid &= point.getZ() < lidarOrigin.getZ() + parameters.nearScanZMaxAboveHead; Point3D center = new Point3D(lidarOrigin.getX(), lidarOrigin.getY(), point.getZ()); valid &= point.distance(center) < parameters.nearScanRadius; return valid; }
@Override protected void setBehaviorInput() { FramePoint3D point = new FramePoint3D(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.getBallLocation().getY(), initialSphereDetectionBehavior.getBallLocation().getZ() + initialSphereDetectionBehavior.getSpehereRadius()); wholeBodyBehavior.setSolutionQualityThreshold(2.01); wholeBodyBehavior.setTrajectoryTime(3); FrameQuaternion tmpOr = new FrameQuaternion(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
public String getID() { String id = ""; id = RobotSide.fromByte(footstepData.getRobotSide()).getSideNameFirstLetter() + "_" + discretize(footstepData.getLocation().getX(), distanceResolution) + "_" + discretize(footstepData.getLocation().getY(), distanceResolution) + "_" + discretize(theta, thetaResolution); return id; } }