/** * Gets the maximum z-coordinate of this bounding box. * * @return the maximum z-coordinate. */ public double getMaxZ() { return maxPoint.getZ(); }
public double getMinZ() { if (minCoordinateDirtyBit) throw new RuntimeException("The bounding box min coordinate is not up to date."); return minCoordinate.getZ(); }
/** * Gets the z-coordinate of a point this plane goes through. * * @return the z-coordinate of this plane's point. * @throws RuntimeException if this plane has not been initialized yet. */ public double getPointZ() { checkHasBeenInitialized(); return point.getZ(); }
private double getAverageHeightOfLeaf() { Point3D averagePoint = leaf.getAveragePoint(); return averagePoint.getZ(); }
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; }
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; }
@Override public boolean isValidNearScan(Point3D point, Point3D lidarOrigin) { boolean valid = super.isValidNearScan(point, lidarOrigin); valid &= point.getZ() > getMidFootPoint().getZ() + parameters.nearScanZMinAboveFeet; // valid &= parameters.nearScanCollisions || robotBoundingBoxes.isValidPoint(lidarOrigin, point); valid &= Math.abs(getAngleToPelvis(point, lidarOrigin)) < parameters.nearScanRadians; return valid; }
@Override public boolean containsNaN() { if (Double.isNaN(position.getX()) || Double.isNaN(position.getY()) || Double.isNaN(position.getZ())) return true; if (Double.isNaN(linearVelocity.getX()) || Double.isNaN(linearVelocity.getY()) || Double.isNaN(linearVelocity.getZ())) return true; return false; }
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 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); } }
public void setInput(PelvisHeightTrajectoryMessage pelvisHeightTrajectoryMessage) { this.outgoingPelvisHeightTrajectoryMessage = pelvisHeightTrajectoryMessage; System.out.println("Pelvis height " + pelvisHeightTrajectoryMessage.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().getLast().getPosition().getZ()); hasInputBeenSet.set(true); }
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); } } }
@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 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 setGoalPosition(Point3D position) { goalSphere.setTranslateX(position.getX()); goalSphere.setTranslateY(position.getY()); goalSphere.setTranslateZ(position.getZ()); }
@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); } };
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(); }
@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); } };