OptimizationProblem(OptimizationSegments optimizationSegments, IEnumerable <CurveSpecification> curveSpecifications) { if (optimizationSegments == null) { throw new ArgumentNullException("segmentManager"); } if (curveSpecifications == null) { throw new ArgumentNullException("curveSpecifications"); } this.optimizationSegments = optimizationSegments; this.curveSpecifications = curveSpecifications; this.curveLength = Terms.Variable("curveLength"); this.pointSpecificationTemplates = ( from pointSpecificationIndex in Enumerable.Range(0, curveSpecifications.Count(curveSpecification => curveSpecification is PointCurveSpecification)) select SpecificationTemplate.CreatePointSpecificationTemplate(optimizationSegments.Segments, pointSpecificationIndex) ) .ToArray(); this.directionSpecificationTemplates = ( from directionSpecificationIndex in Enumerable.Range(0, curveSpecifications.Count(curveSpecification => curveSpecification is DirectionCurveSpecification)) select SpecificationTemplate.CreateDirectionSpecificationTemplate(optimizationSegments.Segments, curveLength, directionSpecificationIndex) ) .ToArray(); this.curvatureSpecificationTemplates = ( from curvatureSpecificationIndex in Enumerable.Range(0, curveSpecifications.Count(curveSpecification => curveSpecification is CurvatureCurveSpecification)) select SpecificationTemplate.CreateCurvatureSpecificationTemplate(optimizationSegments.Segments, curveLength, curvatureSpecificationIndex) ) .ToArray(); IEnumerable <ValueTerm> variables = optimizationSegments.Parameters; ValueTerm segmentLength = Terms.Quotient(curveLength, Terms.Constant(optimizationSegments.Segments.Count())); ValueTerm speedError = Terms.Sum ( from segment in optimizationSegments.Segments let position = Terms.Variable("t") let curve = segment.LocalCurve let speed = curve.Speed.Apply(position) let error = Terms.Square(Terms.Difference(speed, segmentLength)) select Terms.IntegrateTrapezoid(error.Abstract(position), new OrderedRange <double>(0, 1), 100) ); ValueTerm fairnessError = Terms.Sum ( from segment in optimizationSegments.Segments let position = Terms.Variable("t") let curve = segment.LocalCurve let jerk = curve.Jerk.Apply(position) let normal = Terms.Normal(curve.Velocity.Apply(position)) let error = Terms.Square(Terms.DotProduct(jerk, normal)) select Terms.IntegrateTrapezoid(error.Abstract(position), new OrderedRange <double>(0, 1), 100) ); // ValueTerm pointSpecificationError = Terms.Sum // ( // Enumerables.Concatenate // ( // Enumerables.Create(Terms.Constant(0)), // from specificationTemplate in Enumerables.Concatenate(pointSpecificationTemplates) // select Terms.NormSquared // ( // Terms.Difference // ( // specificationTemplate.Constraint.Item, // Terms.Vector(specificationTemplate.Constraint.Ranges.Select(range => range.Start)) // ) // ) // ) // ); // ValueTerm directionSpecificationError = Terms.Sum // ( // Enumerables.Concatenate // ( // Enumerables.Create(Terms.Constant(0)), // from specificationTemplate in Enumerables.Concatenate(directionSpecificationTemplates) // select Terms.NormSquared // ( // Terms.Difference // ( // specificationTemplate.Constraint.Item, // Terms.Vector(specificationTemplate.Constraint.Ranges.Select(range => range.Start)) // ) // ) // ) // ); // ValueTerm curvatureSpecificationError = Terms.Sum // ( // Enumerables.Concatenate // ( // Enumerables.Create(Terms.Constant(0)), // from specificationTemplate in Enumerables.Concatenate(curvatureSpecificationTemplates) // select Terms.NormSquared // ( // Terms.Difference // ( // specificationTemplate.Constraint.Item, // Terms.Vector(specificationTemplate.Constraint.Ranges.Select(range => range.Start)) // ) // ) // ) // ); ValueTerm objectiveValue = Terms.Sum ( Terms.Product(Terms.Exponentiation(segmentLength, Terms.Constant(-2)), Terms.Constant(100000.0), speedError), Terms.Product(Terms.Exponentiation(segmentLength, Terms.Constant(-4)), Terms.Constant(1), fairnessError) // Terms.Product(Terms.Exponentiation(segmentLength, Terms.Constant(0)), Terms.Constant(0.1), pointSpecificationError), // Terms.Product(Terms.Exponentiation(segmentLength, Terms.Constant(1)), Terms.Constant(10), directionSpecificationError), // Terms.Product(Terms.Exponentiation(segmentLength, Terms.Constant(2)), Terms.Constant(100), curvatureSpecificationError) ) .Simplify(); IEnumerable <Constraint <ValueTerm> > constraintValues = ( Enumerables.Concatenate ( from pointSpecificationTemplate in pointSpecificationTemplates select pointSpecificationTemplate.Constraint, from directionSpecificationTemplate in directionSpecificationTemplates select directionSpecificationTemplate.Constraint, from curvatureSpecificationTemplate in curvatureSpecificationTemplates select curvatureSpecificationTemplate.Constraint, from segmentIndex in Enumerable.Range(0, optimizationSegments.Segments.Count() - 1) let segment0CurvePoint = optimizationSegments.Segments.ElementAt(segmentIndex + 0).LocalCurve.Point let segment1CurvePoint = optimizationSegments.Segments.ElementAt(segmentIndex + 1).LocalCurve.Point select Constraints.CreateEquality ( segment0CurvePoint.Apply(Terms.Constant(1)), segment1CurvePoint.Apply(Terms.Constant(0)) ), from segmentIndex in Enumerable.Range(0, optimizationSegments.Segments.Count() - 1) let segment0CurveVelocity = optimizationSegments.Segments.ElementAt(segmentIndex + 0).LocalCurve.Velocity let segment1CurveVelocity = optimizationSegments.Segments.ElementAt(segmentIndex + 1).LocalCurve.Velocity select Constraints.CreateEquality ( segment0CurveVelocity.Apply(Terms.Constant(1)), segment1CurveVelocity.Apply(Terms.Constant(0)) ), from segmentIndex in Enumerable.Range(0, optimizationSegments.Segments.Count() - 1) let segment0CurveAcceleration = optimizationSegments.Segments.ElementAt(segmentIndex + 0).LocalCurve.Acceleration let segment1CurveAcceleration = optimizationSegments.Segments.ElementAt(segmentIndex + 1).LocalCurve.Acceleration select Constraints.CreateEquality ( segment0CurveAcceleration.Apply(Terms.Constant(1)), segment1CurveAcceleration.Apply(Terms.Constant(0)) ) ) ) .ToArray(); Constraint <ValueTerm> constraint = Constraints.Merge(constraintValues); FunctionTerm objectiveFunction = objectiveValue.Abstract(variables); FunctionTerm constraintFunction = constraint.Item.Abstract(variables); this.problem = IpoptProblem.Create(objectiveFunction, constraintFunction, constraint.Ranges); }
static IEnumerable <Substitution> GetSubstitutions(OptimizationSegments optimizationSegments, OptimizationProblem optimizationProblem, double curveLength, IEnumerable <CurveSpecification> curveSpecifications) { yield return(new Substitution(optimizationProblem.CurveLength, Terms.Constant(curveLength))); IEnumerable <PointCurveSpecification> pointCurveSpecifications = curveSpecifications.OfType <PointCurveSpecification>(); IEnumerable <DirectionCurveSpecification> directionCurveSpecifications = curveSpecifications.OfType <DirectionCurveSpecification>(); IEnumerable <CurvatureCurveSpecification> curvatureCurveSpecifications = curveSpecifications.OfType <CurvatureCurveSpecification>(); foreach (int pointSpecificationTemplateIndex in Enumerable.Range(0, optimizationProblem.PointSpecificationTemplates.Count())) { SpecificationTemplate pointSpecificationTemplate = optimizationProblem.PointSpecificationTemplates.ElementAt(pointSpecificationTemplateIndex); if (pointSpecificationTemplateIndex < pointCurveSpecifications.Count()) { PointCurveSpecification pointCurveSpecification = pointCurveSpecifications.ElementAt(pointSpecificationTemplateIndex); ValueTerm position = Terms.Constant(pointCurveSpecification.Position); ValueTerm value = Terms.Constant(pointCurveSpecification.Point.X, pointCurveSpecification.Point.Y); foreach (Substitution substitution in pointSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments, pointCurveSpecification.Position, position, value)) { yield return(substitution); } } else { foreach (Substitution substitution in pointSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments)) { yield return(substitution); } } } foreach (int directionSpecificationTemplateIndex in Enumerable.Range(0, optimizationProblem.DirectionSpecificationTemplates.Count())) { SpecificationTemplate directionSpecificationTemplate = optimizationProblem.DirectionSpecificationTemplates.ElementAt(directionSpecificationTemplateIndex); if (directionSpecificationTemplateIndex < directionCurveSpecifications.Count()) { DirectionCurveSpecification directionCurveSpecification = directionCurveSpecifications.ElementAt(directionSpecificationTemplateIndex); ValueTerm position = Terms.Constant(directionCurveSpecification.Position); ValueTerm value = Terms.Constant(directionCurveSpecification.Direction); foreach (Substitution substitution in directionSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments, directionCurveSpecification.Position, position, value)) { yield return(substitution); } } else { foreach (Substitution substitution in directionSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments)) { yield return(substitution); } } } foreach (int curvatureSpecificationTemplateIndex in Enumerable.Range(0, optimizationProblem.CurvatureSpecificationTemplates.Count())) { SpecificationTemplate curvatureSpecificationTemplate = optimizationProblem.CurvatureSpecificationTemplates.ElementAt(curvatureSpecificationTemplateIndex); if (curvatureSpecificationTemplateIndex < curvatureCurveSpecifications.Count()) { CurvatureCurveSpecification curvatureCurveSpecification = curvatureCurveSpecifications.ElementAt(curvatureSpecificationTemplateIndex); ValueTerm position = Terms.Constant(curvatureCurveSpecification.Position); ValueTerm value = Terms.Constant(curvatureCurveSpecification.Curvature); foreach (Substitution substitution in curvatureSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments, curvatureCurveSpecification.Position, position, value)) { yield return(substitution); } } else { foreach (Substitution substitution in curvatureSpecificationTemplate.GetSubstitutions(optimizationSegments.Segments)) { yield return(substitution); } } } }