/// <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); }