private void initTransition() { camXSpeed = -(camX - storedCameraPositions.get(storedPositionIndex).getX()) / transitionTime; camYSpeed = -(camY - storedCameraPositions.get(storedPositionIndex).getY()) / transitionTime; camZSpeed = -(camZ - storedCameraPositions.get(storedPositionIndex).getZ()) / transitionTime; fixXSpeed = -(fixX - storedFixPositions.get(storedPositionIndex).getX()) / transitionTime; fixYSpeed = -(fixY - storedFixPositions.get(storedPositionIndex).getY()) / transitionTime; fixZSpeed = -(fixZ - storedFixPositions.get(storedPositionIndex).getZ()) / transitionTime; transitioning = true; lastTransitionTime = System.currentTimeMillis(); }
private boolean approximatelyEqual(FootstepDataMessage currentLocation, FootstepDataMessage checkAgainst){ if (currentLocation == null) return false; double xDiff = currentLocation.location.getX() - checkAgainst.location.getX(); double yDiff = currentLocation.location.getY() - checkAgainst.location.getY(); if (currentLocation.robotSide != checkAgainst.robotSide) return false; if (Math.sqrt(Math.pow(xDiff, 2) + Math.pow(yDiff,2)) > 0.05) return false; if (Math.abs(RotationTools.computeYaw(currentLocation.orientation) - RotationTools.computeYaw(checkAgainst.orientation)) > Math.PI/16) return false; return true; }
public void getAllPointsWithinBounds(Box bounds, ArrayList<Point3d> pointsToPack) { for (Point3d point : points) { if (bounds.containsOrEquals(point.getX(), point.getY())) { pointsToPack.add(point); } } }
public static BoundingBox3d union(BoundingBox3d boundingBoxOne, BoundingBox3d boundingBoxTwo) { double minX = Math.min(boundingBoxOne.minPoint.getX() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getX() - boundingBoxTwo.epsilon); double minY = Math.min(boundingBoxOne.minPoint.getY() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getY() - boundingBoxTwo.epsilon); double minZ = Math.min(boundingBoxOne.minPoint.getZ() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getZ() - boundingBoxTwo.epsilon); double maxX = Math.max(boundingBoxOne.maxPoint.getX() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getX() + boundingBoxTwo.epsilon); double maxY = Math.max(boundingBoxOne.maxPoint.getY() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getY() + boundingBoxTwo.epsilon); double maxZ = Math.max(boundingBoxOne.maxPoint.getZ() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getZ() + boundingBoxTwo.epsilon); Point3d unionMin = new Point3d(minX, minY, minZ); Point3d unionMax = new Point3d(maxX, maxY, maxZ); return new BoundingBox3d(unionMin, unionMax, 0.0); }
@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; }
public void setScan(Point3d[] scan) { this.scan = new float[scan.length * 3]; int index = 0; for (Point3d scanPoint : scan) { this.scan[index++] = (float) scanPoint.getX(); this.scan[index++] = (float) scanPoint.getY(); this.scan[index++] = (float) scanPoint.getZ(); } }
public void setStartAndEnd(Point3d startPoint, Point3d endPoint) { this.startX.set(startPoint.getX()); this.startY.set(startPoint.getY()); this.startZ.set(startPoint.getZ()); this.endX.set(endPoint.getX()); this.endY.set(endPoint.getY()); this.endZ.set(endPoint.getZ()); this.vectorX.set(endPoint.getX() - startPoint.getX()); this.vectorY.set(endPoint.getY() - startPoint.getY()); this.vectorZ.set(endPoint.getZ() - startPoint.getZ()); }
@Override protected void setBehaviorInput() { FramePoint point = new FramePoint(ReferenceFrame.getWorldFrame(), grabLocation.getX(), grabLocation.getY(), grabLocation.getZ() + objectRadius); atlasPrimitiveActions.wholeBodyBehavior.setSolutionQualityThreshold(2.01); atlasPrimitiveActions.wholeBodyBehavior.setTrajectoryTime(3); FrameOrientation tmpOr = new FrameOrientation(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); atlasPrimitiveActions.wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
public void update(Point3d pointUnfiltered) { x.update(pointUnfiltered.getX()); y.update(pointUnfiltered.getY()); z.update(pointUnfiltered.getZ()); }
@Override public int hashCode() { return (int) (discretize(footstepData.location.getX(), distanceResolution) + 3424.0 * discretize(footstepData.location.getY(), distanceResolution) + 16353.0 * discretize(theta, thetaResolution) + 72432.0 * footstepData.robotSide.ordinal()); }
public String getID() { String id = ""; id = footstepData.robotSide.getSideNameFirstLetter() + "_" + discretize(footstepData.location.getX(), distanceResolution) + "_" + discretize(footstepData.location.getY(), distanceResolution) + "_" + discretize(theta, thetaResolution); return id; } }
public void setSolePose(Point3d positionInWorld, Quat4d orientationInWorld) { if (transformFromSoleToWorld == null) transformFromSoleToWorld = new RigidBodyTransform(); transformFromSoleToWorld.setRotation(orientationInWorld); transformFromSoleToWorld.setTranslation(positionInWorld.getX(), positionInWorld.getY(), positionInWorld.getZ()); setSolePose(transformFromSoleToWorld); }
public static void applyTranform(Transform transform, Point3d pointToTransform) { Point3D temporaryVector = transform.transform(pointToTransform.getX(), pointToTransform.getY(), pointToTransform.getZ()); pointToTransform.set(temporaryVector.getX(), temporaryVector.getY(), temporaryVector.getZ()); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { return MathTools.isPreciselyBoundedByInclusive(0.0, size.getX(), pointToCheck.getX(), epsilon * 2.0) && MathTools.isPreciselyBoundedByInclusive(-size.getY() / 2.0, size.getY() / 2.0, pointToCheck.getY(), epsilon * 2.0) && MathTools.isPreciselyBoundedByInclusive(0.0, size.getZ(), pointToCheck.getZ(), epsilon * 2.0) && rampPlane.isOnOrBelow(pointToCheck, epsilon); }