public Gap(VelocityConstraint constraint, double v0, double v1, double v2, ActionType action) { Constraint = constraint; this.v0 = v0; this.v1 = v1; this.v2 = v2; Action = action; }
/// <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); } }
/// <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; }
public JointMotionProfile(MotionParameter parameters, VelocityConstraint constraint, params VelocityConstraint[] constraints) : this(parameters, _defaultInitialAcceleration, _defaultInitialVelocity, new List <VelocityConstraint>() { constraint }.Concat(constraints)) { }