public static FunctionTerm BigSum(int dimension, int length)
{
if (dimension < 0)
{
throw new ArgumentOutOfRangeException("dimension");
}
if (length < 0)
{
throw new ArgumentOutOfRangeException("length");
}
ValueTerm seed = Constant(Enumerable.Repeat <double>(0, dimension));
IEnumerable <Variable> variables =
(
from index in Enumerable.Range(0, length)
select new Variable(dimension, string.Format("x_{0}", index))
)
.ToArray();
return(new FunctionDefinition
(
string.Format("big_sum_{0}_{1}", dimension, length),
variables.Aggregate(seed, ApplyVectorSum).Abstract(variables),
new BasicSyntax("Σ")
));
}
// TODO: remove duplication regarding the different specification types
public static SpecificationTemplate CreatePointSpecificationTemplate(IEnumerable <Segment> segments, int index)
{
ValueTerm position = Terms.Variable(string.Format("p_{0}_position", index));
ValueTerm point = Terms.Variable(string.Format("p_{0}_point", index), 2);
IEnumerable <ValueTerm> segmentWeights =
(
from segmentIndex in Enumerable.Range(0, segments.Count())
select Terms.Variable(string.Format("p_{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(segment.GlobalCurve.Point.Apply(position), point)
)
)
)
);
return(new SpecificationTemplate(position, point, segmentWeights, constraint));
}
public override T Rewrite <T>(T term)
{
if (!(term is ValueTerm))
{
return(null);
}
ValueTerm valueTerm = (ValueTerm)(BaseTerm)term;
if (valueTerm is ValueDefinition)
{
return(null);
}
if (valueTerm is Constant)
{
return(null);
}
if (valueTerm is Vector && ((Vector)valueTerm).Terms.Count() != 1 && ((Vector)valueTerm).Terms.All(subTerm => subTerm is Constant))
{
return(null);
}
if (valueTerm.GetFreeVariables().Any())
{
return(null);
}
IEnumerable <double> values = valueTerm.Evaluate();
ValueTerm result = values.Count() == 1 ? (ValueTerm) new Constant(values.Single()) : (ValueTerm) new Vector(values.Select(value => new Constant(value)));
return((T)(BaseTerm)result);
}
public FunctionTermCurve(FunctionTerm function)
{
if (function == null)
{
throw new ArgumentNullException("function");
}
if (function.DomainDimension != 1)
{
throw new ArgumentException("parameter 'function' has wrong domain dimension.");
}
if (function.CodomainDimension != 2)
{
throw new ArgumentException("parameter 'function' has wrong codomain dimension.");
}
this.point = function;
this.velocity = point.GetDerivatives().Single();
this.acceleration = velocity.GetDerivatives().Single();
this.jerk = acceleration.GetDerivatives().Single();
ValueTerm position = Terms.Variable("t");
ValueTerm speedValue = Terms.Norm(velocity.Apply(position));
this.speed = speedValue.Abstract(position);
ValueTerm directionValue = Terms.Angle(velocity.Apply(position));
this.direction = directionValue.Abstract(position);
ValueTerm curvatureValue = Terms.Quotient(direction.GetDerivatives().Single().Apply(position), speedValue);
this.curvature = curvatureValue.Abstract(position);
}
public override ValueTerm Substitute(Variable variable, ValueTerm substitute)
{
return(new Application
(
function.Substitute(variable, substitute),
parameter.Substitute(variable, substitute)
));
}
public override ValueTerm Substitute(Variable variable, ValueTerm substitute)
{
return(new Vector
(
from term in terms
select term.Substitute(variable, substitute)
));
}
static Vector GetSelectionVector(ValueTerm term)
{
return(new Vector
(
from index in Enumerable.Range(0, term.Dimension)
select new Selection(term, index)
));
}
static bool ShouldSwap(ValueTerm term1, ValueTerm term2)
{
if (!(term1 is Constant) && term2 is Constant)
{
return(true);
}
return(false);
}
public AbstractionSyntax(IEnumerable <Variable> variables, ValueTerm term)
{
if (variables == null)
{
throw new ArgumentNullException("variables");
}
if (term == null)
{
throw new ArgumentNullException("term");
}
this.variables = variables;
this.term = term;
}
public SelectionSyntax(ValueTerm term, int index)
{
if (term == null)
{
throw new ArgumentNullException("term");
}
if (index < 0 || index >= term.Dimension)
{
throw new ArgumentOutOfRangeException("index");
}
this.term = term;
this.index = index;
}
static void RunTest <T>(T term, Rule rule) where T : VariableTerm <T>
{
Terminal.Write(string.Format("testing rule {0}...", rule.GetType().Name));
T result = rule.Rewrite(term);
if (result == null)
{
throw new InvalidOperationException();
}
if (term is ValueTerm)
{
if (!(result is ValueTerm))
{
throw new InvalidOperationException();
}
ValueTerm termValueTerm = (ValueTerm)(BaseTerm)term;
ValueTerm resultValueTerm = (ValueTerm)(BaseTerm)result;
if (termValueTerm.Dimension != resultValueTerm.Dimension)
{
throw new InvalidOperationException();
}
}
if (term is FunctionTerm)
{
if (!(result is FunctionTerm))
{
throw new InvalidOperationException();
}
FunctionTerm termFunctionTerm = (FunctionTerm)(BaseTerm)term;
FunctionTerm resultFunctionTerm = (FunctionTerm)(BaseTerm)result;
if (resultFunctionTerm.DomainDimension != termFunctionTerm.DomainDimension)
{
throw new InvalidOperationException();
}
if (resultFunctionTerm.CodomainDimension != termFunctionTerm.CodomainDimension)
{
throw new InvalidOperationException();
}
}
Terminal.Write("success");
Terminal.WriteLine();
}
public Assignment(Variable variable, ValueTerm term)
{
if (variable == null)
{
throw new ArgumentNullException("variable");
}
if (term == null)
{
throw new ArgumentNullException("term");
}
this.variable = variable;
this.term = term;
}
public FirstOrderRule(ValueTerm originalTerm, ValueTerm rewrittenTerm)
{
if (originalTerm == null)
{
throw new ArgumentNullException("originalTerm");
}
if (rewrittenTerm == null)
{
throw new ArgumentNullException("rewrittenTerm");
}
this.originalTerm = originalTerm;
this.rewrittenTerm = rewrittenTerm;
}
public ApplicationSyntax(FunctionTerm function, ValueTerm parameter)
{
if (function == null)
{
throw new ArgumentNullException("function");
}
if (parameter == null)
{
throw new ArgumentNullException("parameter");
}
this.function = function;
this.parameter = parameter;
}
static ValueTerm Reduce(IEnumerable <Variable> variables, ValueTerm term, ValueTerm parameter)
{
IEnumerable <ValueTerm> substitutes = Enumerable.Zip
(
variables.Select(variable => variable.Dimension).GetPartialSums(),
variables.Select(variable => variable.Dimension),
(start, length) => new Vector
(
from index in Enumerable.Range(start, length)
select new Selection(parameter, index)
)
)
.ToArray();
return(term.Substitute(variables, substitutes));
}
public Abstraction(IEnumerable <Variable> variables, ValueTerm term)
{
if (variables == null)
{
throw new ArgumentNullException("variables");
}
if (term == null)
{
throw new ArgumentNullException("term");
}
if (!variables.IsDistinct())
{
throw new ArgumentException("The given variables are not distinct.");
}
this.variables = variables.ToArray();
this.term = term;
}
public Application(FunctionTerm function, ValueTerm parameter)
{
if (function == null)
{
throw new ArgumentNullException("function");
}
if (parameter == null)
{
throw new ArgumentNullException("parameter");
}
if (parameter.Dimension != function.DomainDimension)
{
throw new ArgumentException(string.Format("Dimension mismatch applying function '{0}' to parameter '{1}'.", function, parameter));
}
this.function = function;
this.parameter = parameter;
}
public override T Rewrite <T>(T term)
{
if (!(term is Application))
{
return(null);
}
Application application = (Application)(BaseTerm)term;
if (!(application.Function is Product))
{
return(null);
}
if (!(application.Parameter is Vector))
{
return(null);
}
Vector vector = (Vector)application.Parameter;
if (vector.Terms.Count() != 2)
{
return(null);
}
ValueTerm parameter0 = vector.Terms.ElementAt(0);
ValueTerm parameter1 = vector.Terms.ElementAt(1);
if (parameter0 is Application && ((Application)parameter0).Function is Product)
{
return(null);
}
if (!ShouldSwap(parameter0, parameter1))
{
return(null);
}
return((T)(BaseTerm) new Application(new Product(), new Vector(Enumerables.Create(parameter1, parameter0))));
}
public override FunctionTerm Substitute(Variable variable, ValueTerm substitute)
{
if (variables.Contains(variable))
{
return(this);
}
IEnumerable <Variable> newVariables = FindUnusedVariables
(
variables,
Enumerables.Concatenate
(
GetFreeVariables(),
Enumerables.Create(variable),
substitute.GetFreeVariables()
)
);
ValueTerm newTerm = term.Substitute(variables, newVariables);
return(new Abstraction(newVariables, newTerm.Substitute(variable, substitute)));
}
static IEnumerable <ValueTerm> GetDerivativesExponentiation(ValueTerm term, Variable variable)
{
if (!(term is Application))
{
return(null);
}
Application application = (Application)term;
if (!(application.Function is Exponentiation))
{
return(null);
}
if (!(application.Parameter is Vector))
{
return(null);
}
Vector vector = (Vector)application.Parameter;
if (vector.Terms.Count() != 2)
{
return(null);
}
ValueTerm term0 = vector.Terms.ElementAt(0);
ValueTerm term1 = vector.Terms.ElementAt(1);
return(Enumerable.Zip
(
term0.GetDerivatives(variable),
term1.GetDerivatives(variable),
(derivative0, derivative1) => Term.Sum
(
Term.Product(term1, Term.Exponentiation(term0, Term.Difference(term1, Term.Constant(1))), derivative0),
Term.Product(Term.Logarithm(term0), Term.Exponentiation(term0, term1), derivative1)
)
));
}
public static FunctionTerm BigProduct(int length)
{
if (length < 0)
{
throw new ArgumentOutOfRangeException("length");
}
ValueTerm seed = Constant(1);
IEnumerable <Variable> variables =
(
from index in Enumerable.Range(0, length)
select new Variable(1, string.Format("x_{0}", index))
)
.ToArray();
return(new FunctionDefinition
(
string.Format("big_product_{0}", length),
variables.Aggregate(seed, ApplyProduct).Abstract(variables),
new BasicSyntax("π")
));
}
public ValueDefinition(string name, ValueTerm value, Syntax syntax)
{
if (name == null)
{
throw new ArgumentNullException("name");
}
if (value == null)
{
throw new ArgumentNullException("value");
}
if (value.GetFreeVariables().Any())
{
throw new ArgumentException("cannot create definition containing free variables.");
}
if (syntax == null)
{
throw new ArgumentNullException("syntax");
}
this.name = name;
this.value = value;
this.syntax = syntax;
}
public override T Rewrite <T>(T term)
{
if (!(term is ValueTerm))
{
return(null);
}
ValueTerm valueTerm = (ValueTerm)(BaseTerm)term;
IEnumerable <Assignment> equations = GetEquations(originalTerm, valueTerm);
if (equations == null)
{
return(null);
}
if (!Substitution.IsAssignment(equations))
{
return(null);
}
return((T)(BaseTerm)Substitution.FromEquations(equations).Apply(rewrittenTerm));
}
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));
}
static IEnumerable <ValueTerm> GetDerivativesSum(ValueTerm term, Variable variable)
{
if (!(term is Application))
{
return(null);
}
Application application = (Application)term;
if (!(application.Function is Sum))
{
return(null);
}
if (!(application.Parameter is Vector))
{
return(null);
}
Vector vector = (Vector)application.Parameter;
if (vector.Terms.Count() != 2)
{
return(null);
}
ValueTerm term0 = vector.Terms.ElementAt(0);
ValueTerm term1 = vector.Terms.ElementAt(1);
return(Enumerable.Zip
(
term0.GetDerivatives(variable),
term1.GetDerivatives(variable),
(derivative0, derivative1) => Term.Sum(derivative0, derivative1)
));
}
SpecificationTemplate(ValueTerm position, ValueTerm value, IEnumerable <ValueTerm> segmentWeights, Constraint <ValueTerm> constraint)
{
if (position == null)
{
throw new ArgumentNullException("position");
}
if (value == null)
{
throw new ArgumentNullException("value");
}
if (segmentWeights == null)
{
throw new ArgumentNullException("segmentWeights");
}
if (constraint == null)
{
throw new ArgumentNullException("constraint");
}
this.position = position;
this.value = value;
this.segmentWeights = segmentWeights;
this.constraint = constraint;
}
public FunctionTermCurve TransformInput(FunctionTerm transformation)
{
ValueTerm position = Terms.Variable("t");
return(new FunctionTermCurve(point.Apply(transformation.Apply(position)).Abstract(position)));
}
public override ValueTerm Substitute(Variable variable, ValueTerm substitute)
{
return(variable == this ? substitute : this);
}
public FunctionTermCurve InstantiateParameter(ValueTerm parameter)
{
ValueTerm position = Terms.Variable("t");
return(new FunctionTermCurve(function.Apply(position, parameter).Abstract(position)));
}
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);
}