code: public Pose interpolate(Pose p2, double ratio) {
  		double newX = lerp(getX(),p2.getX(),ratio);
  		double newY = lerp(getY(),p2.getY(),ratio);
  		if (Double.isNaN(this.z)) {
  			double newTheta = lerpDegrees(getTheta(),p2.getTheta(),ratio);
  			return new Pose(newX,newY,newTheta);
  		}
  		double newZ = lerp(getZ(),p2.getZ(),ratio);
  		double newRoll = lerpDegrees(getRoll(),p2.getRoll(),ratio);
  		double newPitch = lerpDegrees(getPitch(),p2.getPitch(),ratio);
  		double newYaw = lerpDegrees(getYaw(),p2.getYaw(),ratio);
  		return new Pose(newX,newY,newZ,newRoll,newPitch,newYaw);
  	}
docs: Computes the {@link Pose} between this {@link Pose} and a given {@link Pose} via bilinear interpolation.
  @param p2 The second {@link Pose} used for interpolation.
  @param ratio Parameter in [0,1] used for bilinear interpolation.
  @return The {@link Pose} between this {@link Pose} and a given {@link Pose} via bilinear interpolation.