
	 * @return The temporal profile of a portion of this this {@link Trajectory}.
	 */
	public double[] getDts(int from, int to) {
		double[] ret = new double[to-from];
		for (int i = from; i < to; i++) {
			ret[i-from] = this.dts[i];
		}
		return ret;
	}

	private double getConstantAcceleration(double percentComplete) {
//		if (percentComplete <= 0.5) return Gaussian.Phi(percentComplete, 0.25, 0.3)*MAX_ACCELERATION;
//		return -Gaussian.Phi(percentComplete, 0.75, 0.3)*MAX_ACCELERATION;
		if (percentComplete <= 0.25) return MAX_ACCELERATION;
		if (percentComplete <= 0.78) return 0.0;
		return -MAX_ACCELERATION;
	}

	private double getLinearAcceleration(double percentComplete) {
//		if (percentComplete <= 0.5) return Gaussian.Phi(percentComplete, 0.25, 0.3)*MAX_ACCELERATION;
//		return -Gaussian.Phi(percentComplete, 0.75, 0.3)*MAX_ACCELERATION;
		if (percentComplete <= 0.5) return 2*percentComplete*MAX_ACCELERATION;
		return -(percentComplete-0.5)*MAX_ACCELERATION;
	}

	private double getAcceleration(double percentComplete) {
//		return getLinearAcceleration(percentComplete);
		return getConstantAcceleration(percentComplete);
	}
	
	/**
	 * Get the sequence number of the path point that is closest to a 
	 * given coordinate.
	 * @param coord The coordinate to which the closest sequence number should be found.
	 * @return The sequence number of the path point that is closest to the given coordinate.
	 */
	public int getSequenceNumber(Coordinate coord) {
		int minIndex = 0;
		double minDist = Double.MAX_VALUE;
		for (int i = 0; i < this.getPositions().length; i++) {
			if (this.getPositions()[i].distance(coord) < minDist) {
				minDist = this.getPositions()[i].distance(coord);
				minIndex = i;
			}
		}
		return minIndex;
	}

	/**
	 * Get an estimate of the time left to move on this {@link Trajectory}
	 * given the current position. The estimate is based on this {@link Trajectory}'s
	 * temporal profile.
	 * @param positionNow The position from which the estimate should be computed.
	 * @return An estimate of the time left to move on this {@link Trajectory}
	 * given the current position.
	 */
	public double getTimeLeftEstimate(Coordinate positionNow) {
		int minIndex = 0;
		double minDist = Double.MAX_VALUE;
		for (int i = 0; i < this.getPositions().length; i++) {
			if (this.getPositions()[i].distance(positionNow) < minDist) {
				minDist = this.getPositions()[i].distance(positionNow);
				minIndex = i;
			}
		}
		return getTimeLeftEstimate(minIndex);
	}

	
	/**
	 * Get an estimate of the time left to move on this {@link Trajectory}
	 * between two path indices. The estimate is based on this {@link Trajectory}'s
	 * temporal profile.
	 * @param sequenceNumNow The path index from which the estimate should be computed.
	 * @param sequenceNumTo The path index to which the estimate should be computed.
	 * @return An estimate of the time left to move on this {@link Trajectory}
	 * given the current path index.
	 */
	public double getTimeToEstimate(int sequenceNumNow, int sequenceNumTo) {
		double timeCounter = 0.0;
		for (int i = sequenceNumNow-this.sequenceNumberStart; i <= sequenceNumTo; i++) {
			timeCounter += this.getDTs()[i];
		}
		return timeCounter;
	}
	
	/**
	 * Get an estimate of the time left to move on this {@link Trajectory}
	 * given the current path index. The estimate is based on this {@link Trajectory}'s
	 * temporal profile.
	 * @param sequenceNum The path index from which the estimate should be computed.
	 * @return An estimate of the time left to move on this {@link Trajectory}
	 * given the current path index.
	 */
	public double getTimeLeftEstimate(int sequenceNum) {
		double timeCounter = 0.0;
		for (int i = sequenceNum-this.sequenceNumberStart; i < this.getDTs().length; i++) {
			timeCounter += this.getDTs()[i];
		}
		return timeCounter;
	}
	
//	public double getTimeLeftEstimate(int sequenceNum, int endIndex) {
//		double timeCounter = 0.0;
//		for (int i = sequenceNum; i < endIndex; i++) {
//			timeCounter += this.getDTs()[i];
//		}
//		return timeCounter;
//	}

	private void updateDts() {
		double dt = 0.1;
		ArrayList<Double> u = new ArrayList<Double>();
		ArrayList<Double> s = new ArrayList<Double>();
		//u = u + a * dt;
		//s = s + u * dt;
		s.add(0.0);
		u.add(0.0);
		double totDistance = 0.0;
		for (int i = 1; i < dts.length; i++) {
			Coordinate prev = getPositions()[i-1];
			Coordinate current = getPositions()[i];
			totDistance += prev.distance(current);
		}
		while (s.get(s.size()-1) < totDistance) {
			double percentComplete = 0.01;
			double newPercent = s.get(s.size()-1)/totDistance;
			if (newPercent > percentComplete) percentComplete = newPercent;
//			System.out.println(percentComplete*100 + "% --> " + getAcceleration(percentComplete));
			u.add(u.get(u.size()-1)+getAcceleration(percentComplete)*dt);
			s.add(s.get(s.size()-1)+u.get(u.size()-1)*dt);
		}
		totDistance = 0.0;
		dts[0] = 0.0;
		double prevSum = 0.0;
		for (int i = 1; i < dts.length; i++) {
			Coordinate prev = getPositions()[i-1];
			Coordinate current = getPositions()[i];
			totDistance += prev.distance(current);
			int countDts = 0;
			for (int j = 0; j < s.size(); j++) {
				if (s.get(j) > totDistance) {
					countDts = j-1;
					break;
				}
			}
			prevSum += dts[i-1];
			dts[i] = Math.max(countDts*dt-prevSum,0.001);
		}
	}
	
	/**
	 * Set the minimum transition times between path poses
	 * of this {@link Trajectory}. This is essentially the fastest
	 * time profile of this {@link Trajectory}.
	 * @param dts A vector containing the minimum transition times between path poses
	 * of this {@link Trajectory}. Values are assumed to be in seconds. 
	 */
	public void setDTs(double[] dts) {
		this.dts = dts;
	}
	
//	public double getMinTraversalTime() {
//		return dts[dts.length-1];
//	}

	/**
	 * Get the path (list of {@link PoseSteering}s) of this {@link Trajectory}.
	 * @return The path (list of {@link PoseSteering}s) of this {@link Trajectory}.
	 */
	public PoseSteering[] getPoseSteering() {
		return psa;
	}
	
	/**
	 * Get the path (list of {@link Pose}s) of this {@link Trajectory}.
	 * @return The path (list of {@link Pose}s) of this {@link Trajectory}.
	 */
	public Pose[] getPose() {
		Pose[] ret = new Pose[psa.length];
		for (int i = 0; i < ret.length; i++) {
			ret[i] =