/// <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 JointMotionProfile(JointMotionProfileInputSet inputSet)
{
InputSet = inputSet;
OriginalConstraints = new ConstraintsCollection(inputSet.Constraints.Select(c => c.Copy()));
EffectiveConstraints = inputSet.Constraints.GetEffectiveConstraints();
#if DEBUG
EffectiveConstraintsHistory = new List <ConstraintsCollection>
{
new ConstraintsCollection(EffectiveConstraints.Select(ec => ec.Copy()))
};
CloseHighTightGapsIteratively();
while (true)
{
EffectiveConstraintsHistory.Add(new ConstraintsCollection(EffectiveConstraints.Select(ec => ec.Copy())));
if (CalculateProfile())
{
break;
}
else
{
// possibility to set brakepoint here
NumRecalculations++;
if (EffectiveConstraints.Any(ec => ec.MaximumVelocity == 0))
{
throw new JointMotionCalculationException($"Invalid Effective Constraint", inputSet);
}
}
}
#else
while (!CalculateProfile())
{
}
#endif
Timestamps = CalculateTimestampsAtConstraintOriginalDistances().ToList();
}
/// <summary>
/// (Iteratively) merges the given constraint with the previous constraint the the lower MaxVel of both.
/// </summary>
/// <param name="constraint">Constraint to merge with its previous constraint</param>
/// <param name="index">Index of the given constraint within the EffectiveConstraints list</param>
/// <param name="forceMerge">Forces a complete merge without iteratively approaching to the previous constraint</param>
/// <returns>True if constraint was completely merged, otherwise false</returns>
private bool MergeWithPreviousConstraint(VelocityConstraint constraint, int index, bool forceMerge = false)
{
const double reduceByDistance = 100; // mm
const double reduceByVelocity = 50; // mm/s
if (index == 0)
{
// first constraint -> no previous constraint -> try reducing velocity for reachability
constraint.ReduceBy(reduceByVelocity);
}
else
{
var previousConstraint = EffectiveConstraints[index - 1];
if (!forceMerge && previousConstraint > constraint && previousConstraint.Length > reduceByDistance)
{
// The previous constraint is higher than the given constraint. As it is not allowed
// to increase the MaxVel of a constraint, the MaxVel of the previous constraint must be reduced.
// Because the previous constraint may be a very long one, a complete merge at one time could waste
// precious "high velocity time". Therefore, the merging is done iteratively.
constraint.Start -= reduceByDistance;
constraint.Length += reduceByDistance;
previousConstraint.Length -= reduceByDistance;
}
else if (!forceMerge && previousConstraint < constraint && Math.Abs(previousConstraint - constraint) > reduceByVelocity)
{
// The previous constraint is below the given constraint. Therefore, the given constraints MaxVelo
// must be reduced. Because that could be a waste of "high velocity time", this is done interatively.
constraint.ReduceBy(reduceByVelocity);
}
else
{
// Either foreMerge is true or the constraint which should be reduced is not long / high enough anymore
// -> now completely merge the constraints by removing the given constraint and adding its length to the previous constraint
// To be safe, the minimum of the both MaxVels is taken
EffectiveConstraints.RemoveAt(index);
previousConstraint.Length += constraint.Length;
previousConstraint.MaximumVelocity = Math.Min(constraint.MaximumVelocity, previousConstraint.MaximumVelocity);
return(true);
}
}
return(false);
}
/// <summary>
/// Removes the given constraint, moves the start of the next constraint to the start of the
/// given constraint and adds its length to the next constraint.
/// </summary>
private void MergeWithNextConstraint(VelocityConstraint constraint, int index)
{
EffectiveConstraints.RemoveAt(index);
EffectiveConstraints[index].Start -= constraint.Length;
EffectiveConstraints[index].Length += constraint.Length;
}
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);
}
