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))
{
}
