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