Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
        /// <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);
            }
        }
Exemplo n.º 3
0
        /// <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);
        }
Exemplo n.º 4
0
 /// <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;
 }
Exemplo n.º 5
0
 public JointMotionProfile(MotionParameter parameters, VelocityConstraint constraint, params VelocityConstraint[] constraints)
     : this(parameters, _defaultInitialAcceleration, _defaultInitialVelocity, new List <VelocityConstraint>() { constraint }.Concat(constraints))
 {
 }