static IEnumerable <ValueTerm> GetValues(int segmentCount, FunctionTermCurveTemplate segmentTemplate, IEnumerable <double> values)
{
return
((
from segmentIndex in Enumerable.Range(0, segmentCount)
select Terms.Constant(values.Skip(segmentIndex * segmentTemplate.ParameterDimension).Take(segmentTemplate.ParameterDimension))
)
.ToArray());
}
public IEnumerable <Substitution> GetSubstitutions(IEnumerable <Segment> segments)
{
yield return(new Substitution(position, Terms.Constant(0)));
yield return(new Substitution(value, Terms.Constant(Enumerable.Repeat(0.0, value.Dimension))));
foreach (int segmentIndex in Enumerable.Range(0, segments.Count()))
{
yield return(new Substitution(segmentWeights.ElementAt(segmentIndex), Terms.Constant(0)));
}
}
public static SpecificationTemplate CreateCurvatureSpecificationTemplate(IEnumerable <Segment> segments, ValueTerm speed, int index)
{
ValueTerm position = Terms.Variable(string.Format("c_{0}_position", index));
ValueTerm curvature = Terms.Variable(string.Format("c_{0}_curvature", index), 1);
IEnumerable <ValueTerm> segmentWeights =
(
from segmentIndex in Enumerable.Range(0, segments.Count())
select Terms.Variable(string.Format("c_{0}_segment_weight_{1}", index, segmentIndex))
)
.ToArray();
Constraint <ValueTerm> constraint = Constraints.CreateZero
(
Terms.Sum
(
Enumerable.Zip
(
segments,
segmentWeights,
(segment, segmentWeight) => Terms.Scaling
(
segmentWeight,
Terms.Difference
(
Terms.Scaling
(
Terms.Exponentiation(speed, Terms.Constant(-3)),
Terms.DotProduct(segment.GlobalCurve.Acceleration.Apply(position), Terms.Normal(segment.GlobalCurve.Velocity.Apply(position)))
),
curvature
)
)
)
)
);
return(new SpecificationTemplate(position, curvature, segmentWeights, constraint));
}
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 Vector2Double EvaluateVector(FunctionTerm function, double value)
{
IEnumerable <double> result = function.Apply(Terms.Constant(value)).Evaluate();
return(new Vector2Double(result.ElementAt(0), result.ElementAt(1)));
}
static double EvaluateScalar(FunctionTerm function, double value)
{
IEnumerable <double> result = function.Apply(Terms.Constant(value)).Evaluate();
return(result.Single());
}
public bool Contains(double position)
{
double localPosition = positionTransformation.Apply(Terms.Constant(position)).Evaluate().Single();
return(new OrderedRange <double>(0, 1).Contains(localPosition));
}
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);
}
}
}
}
static IEnumerable <Segment> GetSegments(int segmentCount, FunctionTermCurveTemplate segmentTemplate, IEnumerable <ValueTerm> parameters)
{
return
((
from segmentIndex in Enumerable.Range(0, segmentCount)
let parameter = parameters.ElementAt(segmentIndex)
let curve = segmentTemplate.InstantiateParameter(parameter)
let position = Terms.Variable("t")
let positionTransformation = Terms.Difference(Terms.Product(Terms.Constant(segmentCount), position), Terms.Constant(segmentIndex)).Abstract(position)
select new Segment(curve, positionTransformation)
)
.ToArray());
}
public IEnumerable <Substitution> GetSubstitutions(IEnumerable <Segment> segments, double specificationPosition, ValueTerm substitutionPosition, ValueTerm substitutionValue)
{
yield return(new Substitution(position, substitutionPosition));
yield return(new Substitution(value, substitutionValue));
foreach (int segmentIndex in Enumerable.Range(0, segments.Count()))
{
Segment segment = segments.ElementAt(segmentIndex);
double segmentWeight = segment.Contains(specificationPosition) ? 1 : 0;
yield return(new Substitution(segmentWeights.ElementAt(segmentIndex), Terms.Constant(segmentWeight)));
}
}