public JointMotionProfileInputSet(MotionParameter parameters, double initialAcceleration, double initialVelocity, ConstraintsCollection constraints)
{
Parameters = parameters;
InitialAcceleration = initialAcceleration;
InitialVelocity = initialVelocity;
Constraints = constraints;
}
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>
/// Calculates the effective constraints of the ConstraintsCollection. Constraints or part of
/// constraints which are above other constraints (and therefore will never be reached by a
/// velocity profile) are removed and merged into a closed profile of constraints.
/// </summary>
/// <returns>A new ConstraintCollection with no overlapping constraints</returns>
public ConstraintsCollection GetEffectiveConstraints()
{
var distanceValues = this
.SelectMany(s => new[] { s.Start, s.End })
.Distinct()
.OrderBy(d => d)
.ToList();
var minPoints = new List <ConstraintPoint>();
foreach (var distance in distanceValues)
{
var containingSegments = this.Where(s => s.Contains(distance));
if (containingSegments.Any())
{
var minAllowedVelocity = containingSegments.Min(s => s.MaximumVelocity);
minPoints.Add(new ConstraintPoint(distance, minAllowedVelocity));
}
}
var effectiveConstraints = new ConstraintsCollection();
for (var i = 0; i < minPoints.Count; i++)
{
var from = minPoints[i];
var endIndex = minPoints.FindIndex(i + 1, sp => sp.Velocity != from.Velocity);
if (endIndex == -1)
{
// constraint with other velocity further found
effectiveConstraints.Add(new VelocityConstraint(from.Distance, this.Max(s => s.End) - from.Distance, from.Velocity));
return(effectiveConstraints);
}
else
{
var to = minPoints[endIndex];
effectiveConstraints.Add(new VelocityConstraint(from.Distance, to.Distance - from.Distance, from.Velocity));
}
i = endIndex - 1;
}
return(effectiveConstraints);
}
public JointMotionProfile(MotionParameter parameters, ConstraintsCollection constraints)
: this(parameters, _defaultInitialAcceleration, _defaultInitialVelocity, constraints)
{
}
public JointMotionProfile(MotionParameter parameters, double initialAcceleration, double initialVelocity, ConstraintsCollection constraints)
: this(new JointMotionProfileInputSet(parameters, initialAcceleration, initialVelocity, constraints))
{
}
