public IEnumerable <DistanceTimestamp> CalculateTimestamps(IEnumerable <double> distances)
{
var distanceValues = distances
.Distinct()
.OrderBy(d => d)
.ToList();
var rampIndex = 0;
var timesSum = 0.0;
foreach (var distance in distanceValues)
{
while (RampResults[rampIndex].EndDistance < distance)
{
timesSum += RampResults[rampIndex].TotalDuration;
rampIndex++;
if (rampIndex > RampResults.Count - 1)
{
rampIndex--;
break;
}
}
if (RampResults[rampIndex].Direction == RampDirection.Constant)
{
yield return(new DistanceTimestamp(distance, timesSum + (distance - RampResults[rampIndex].StartDistance) / RampResults[rampIndex].vFrom));
}
else
{
var t = ExtendedP2PCalculator.GetTimeAt(RampResults[rampIndex], distance - RampResults[rampIndex].StartDistance);
yield return(new DistanceTimestamp(distance, timesSum + t));
}
}
}
/// <summary>
/// Gets the status at the given time [s]
/// </summary>
/// <param name="t">Time in seconds within the profile. Must be >= 0 and <= TotalDuration</param>
/// <param name="v">Velocity [mm/s] at the given time</param>
/// <param name="s">Distance [mm] at the given time</param>
public void GetStatus(double t, out double a, out double v, out double s)
{
t = Math.Min(t, TotalDuration);
var pointToIndex = TimesAtVelocityPoints.FindIndex(tAtPoint => tAtPoint > t) + 1;
if (pointToIndex == 0)
{
pointToIndex = VelocityProfilePoints.Count - 1;
}
var pointFromIndex = pointToIndex - 1;
var pointFrom = VelocityProfilePoints[pointFromIndex];
var pointTo = VelocityProfilePoints[pointToIndex];
var tFrom = TimesAtVelocityPoints.ElementAtOrDefault(pointFromIndex - 1);
var tInRamp = t - tFrom;
if (pointFrom.Velocity == pointTo.Velocity)
{
v = pointFrom.Velocity;
s = tInRamp * v;
a = 0.0;
}
else
{
var rampresult = RampResults.ElementAtOrDefault(pointFromIndex);
ExtendedP2PCalculator.CalculateStatus(rampresult, tInRamp, out _, out a, out v, out s);
}
s += pointFrom.Distance;
}
/// <summary>
/// Removes / closes constraint where the velocity profile will never be
/// able to enter above the minimnum of velocities (v0, v1 and v2). Can be closed
/// upfront to save iterations in the more complex CalculateProfile method.
/// Attention: for closing all gaps, you need to call the method iteratively, because
/// new gaps can emerge when existing gaps are closed! Use <see cref="CloseHighTightGapsIteratively" /> method therefore.
/// </summary>
/// <returns>True if all gaps were closed, otherwise false.</returns>
private bool CloseHighTightGaps()
{
var highTightGaps = new List <Gap>();
for (var i = 0; i < EffectiveConstraints.Count; i++)
{
var constraint = EffectiveConstraints[i];
var v0 = i > 0 ? EffectiveConstraints[i - 1].MaximumVelocity : 0;
var v1 = constraint.MaximumVelocity;
var v2 = i < EffectiveConstraints.Count - 1 ? EffectiveConstraints[i + 1].MaximumVelocity : 0;
if (v1 > v0 && v1 > v2)
{
var distanceAcc = ExtendedP2PCalculator.CalculateDistanceNeeded(v0, v1, Parameters);
var distanceDec = ExtendedP2PCalculator.CalculateDistanceNeeded(v1, v2, Parameters);
if (distanceAcc + distanceDec > constraint.Length)
{
var distanceAccToV2 = ExtendedP2PCalculator.CalculateDistanceNeeded(v0, v2, Parameters);
if (distanceAccToV2 < constraint.Length)
{
highTightGaps.Add(new Gap(constraint, v0, v1, v2, Gap.ActionType.DoNothing));
}
else
{
if (v0 > v2)
{
highTightGaps.Add(new Gap(constraint, v0, v1, v2, Gap.ActionType.MergeWithPrevious));
}
else
{
highTightGaps.Add(new Gap(constraint, v0, v1, v2, Gap.ActionType.MergeWithNext));
}
}
}
}
}
var gapsOrdered = highTightGaps.OrderByDescending(g => g.Constraint.MaximumVelocity);
foreach (var gap in gapsOrdered)
{
switch (gap.Action)
{
case Gap.ActionType.MergeWithPrevious:
MergeWithPreviousConstraint(gap.Constraint, EffectiveConstraints.IndexOf(gap.Constraint), true);
break;
case Gap.ActionType.MergeWithNext:
MergeWithNextConstraint(gap.Constraint, EffectiveConstraints.IndexOf(gap.Constraint));
break;
case Gap.ActionType.DoNothing:
default:
break;
}
}
return(highTightGaps.Any(g => g.Action != Gap.ActionType.DoNothing));
}
public static double GetBrakingDistance(this SimpleP2PCalculator calculator, double t)
{
calculator.GetStatus(t, out _, out var a, out var v, out _);
var motionParameter = new MotionParameter(calculator.JerkMax, -calculator.JerkMax, calculator.AccelerationMax, -calculator.AccelerationMax);
var rampResult = ExtendedP2PCalculator.Calculate(a, v, 0, motionParameter);
return(rampResult.Length);
}
/// <summary>
/// If the constraint's MaxVel is to high to be accelerated to from v0 and then be
/// decellerated from to v2 (all within the available distance), this method tries to
/// iteratively reduce the MaxVel of the constraint, until it can be reached from
/// acceleration as well from decelleration side.
/// </summary>
/// <param name="v0">Velocity at which the given constraint is entered</param>
/// <param name="constraint">The constraint for which a solution is searched</param>
/// <param name="v2">Velocity at which the constraint will be left</param>
/// <param name="availableDistance">Distance which is available for reaching and leaving the constraint</param>
/// <param name="startDistance">Absolute distance from the beginning of the profile until the start of the constraint</param>
/// <param name="velocityPoints">List of previously added velocityPoints</param>
/// <returns>True if stepped down velocity was found, otherwise false</returns>
private bool IterativlyFindSteppedDownVelocity(double a0, double v0, VelocityConstraint constraint, double v2, double startDistance, List <VelocityPoint> velocityPoints)
{
const double stepDownSize = 5.0;
var v1 = constraint.MaximumVelocity;
var limitVelocity = Math.Max(v0, v2);
while (v1 > limitVelocity)
{
if (TryAddVelocityPoints(v1))
{
// successfully found a new targetVelocity which allows
// for accelerating and braking
return(true);
}
v1 -= stepDownSize;
}
// failed to add velocityPoints with any intermediate velocity
// -> try with lastVelocity last time because we may have overstepped that critial point
return(TryAddVelocityPoints(limitVelocity));
bool TryAddVelocityPoints(double v)
{
var a0ToUse = v2 < v0 && v == v0 ? a0 : 0;
var distanceForSDAcc = ExtendedP2PCalculator.CalculateDistanceNeeded(a0, v0, v, Parameters);
var distanceForBrakingFromSD = ExtendedP2PCalculator.CalculateDistanceNeeded(a0ToUse, v, v2, Parameters);
if (distanceForSDAcc + distanceForBrakingFromSD < constraint.Length)
{
// constant velocity will be reached
velocityPoints.Add(new VelocityPoint(startDistance + distanceForSDAcc, a0ToUse, v, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + (constraint.Length - distanceForBrakingFromSD), a0ToUse, v, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, a0ToUse, v2, constraint));
return(true);
}
else if (distanceForSDAcc + distanceForBrakingFromSD == constraint.Length)
{
// constant velocity will not be reached but maximum velocity => exact peak
velocityPoints.Add(new VelocityPoint(startDistance + distanceForSDAcc, a0ToUse, v, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, a0ToUse, v2, constraint));
return(true);
}
else if (v == v0 && Math.Abs(constraint.Length - distanceForBrakingFromSD) < 0.01)
{
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, a0ToUse, v2, constraint));
return(true);
}
return(false);
}
}
private bool CalculateProfile()
{
var velocityPoints = new List <VelocityPoint>()
{
new VelocityPoint(0, InitialAcceleration, InitialVelocity, null)
};
for (var i = 0; i < EffectiveConstraints.Count; i++)
{
var constraint = EffectiveConstraints[i];
var nextConstraint = EffectiveConstraints.ElementAtOrDefault(i + 1);
var a0 = i == 0 ? InitialAcceleration : 0.0;
var v0 = velocityPoints.Last().Velocity;
var v1 = constraint.MaximumVelocity;
var v2 = nextConstraint?.MaximumVelocity ?? 0.0;
var startDistance = velocityPoints.Max(v => v.Distance);
var situation = GetSituation(v0, constraint.MaximumVelocity, nextConstraint?.MaximumVelocity ?? 0);
if (situation == 3)
{
var distanceForAcc = ExtendedP2PCalculator.CalculateDistanceNeeded(a0, v0, v1, Parameters);
if (distanceForAcc < constraint.Length)
{
velocityPoints.Add(new VelocityPoint(startDistance + distanceForAcc, v1, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v1, constraint));
}
else
{
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
return(false);
}
}
else if (situation == 4 || situation == 5)
{
throw new JointMotionCalculationException($"Situation {situation} must not appear! Something went wrong before");
}
else if (situation == 7)
{
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v1, constraint));
}
else if (situation == 8)
{
var brakeDistance = ExtendedP2PCalculator.CalculateDistanceNeeded(v1, v2, Parameters);
if (brakeDistance <= constraint.Length)
{
velocityPoints.Add(new VelocityPoint(startDistance + (constraint.Length - brakeDistance), v1, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v2, constraint));
}
else
{
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
return(false);
}
}
else if (!IterativlyFindSteppedDownVelocity(a0, v0, constraint, v2, startDistance, velocityPoints))
{
switch (situation)
{
case 1:
MergeWithNextConstraint(constraint, i);
break;
case 2:
case 6:
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
break;
default:
throw new JointMotionCalculationException($"Unhandled situation id {situation}");
}
return(false);
}
}
// remove ProfilePoints with same distance and velocity
velocityPoints = velocityPoints.DistinctBy(pp => new { pp.Distance, pp.Velocity }).ToList();
// calculate ramp results and times
var rampResults = new List <ExtendedRampCalculationResult>();
var times = new List <double>();
var timeSum = 0.0;
for (var i = 0; i < velocityPoints.Count - 1; i++)
{
var pFrom = velocityPoints[i];
var pTo = velocityPoints[i + 1];
var ramp = new ExtendedRampCalculationResult(ExtendedP2PCalculator.Calculate(pFrom.Acceleration, pFrom.Velocity, pTo.Velocity, Parameters), pFrom.Distance, timeSum);
var duration = ramp.Direction == RampDirection.Constant ? (pTo.Distance - pFrom.Distance) / pTo.Velocity : ramp.TotalDuration;
if (ramp.Direction == RampDirection.Constant)
{
ramp.Length = pTo.Distance - pFrom.Distance;
ramp.TotalDuration = duration;
}
rampResults.Add(ramp);
if (ramp.Direction != RampDirection.Constant && Math.Abs(ramp.Length - (pTo.Distance - pFrom.Distance)) > 1e-8)
{
throw new JointMotionCalculationException($"Calculated distance differs from velocityPoints distance", InputSet);
}
if (double.IsNaN(duration) || double.IsInfinity(duration))
{
throw new JointMotionCalculationException($"Invalid duration ({duration}) on point {i} at {pFrom.Distance}", InputSet);
}
timeSum += duration;
times.Add(timeSum);
}
VelocityProfilePoints = velocityPoints;
RampResults = rampResults;
TimesAtVelocityPoints = times;
TotalDuration = timeSum;
if (double.IsNaN(TotalDuration) || double.IsInfinity(TotalDuration))
{
throw new JointMotionCalculationException($"Invalid TotalDuration ({TotalDuration})");
}
return(true);
}
