public object ToMatrix(SparseTrajTensor tensor)
{
// one row per quantity, one column per constraint
var totalSize = TDef.TotalTrajectorySize;
var nq = TDef.TotalQuantityCount;
StringBuilder sb = new StringBuilder(nq * 20);
TrajectoryInstance colContent = new TrajectoryInstance(this);
sb.Append("[");
if (tensor.Count > 0)
{
var c1 = tensor.First().Key.Constraint;
colContent.Reset(c1.NDims);
var lastCol = c1.Index;
bool added = false;
foreach (var entry in tensor)
{
var nDims = entry.Key.Constraint.NDims;
var col = entry.Key.Constraint.Index;
var row = entry.Key.Q.GlobalIndex;
if (col != lastCol)
{
if (added)
{
added = false;
sb.Append(colContent.ToString()); // append column to matrix
sb.Append(",...\n");
}
else
{
--lastCol;
}
colContent.Reset(nDims);
// fill in empty columns
if (col - lastCol > 1)
{
sb.Append(M.Zeros(totalSize, col - lastCol + 1));
sb.Append(",...\n");
}
lastCol = col;
}
if (entry.Value.Count > 1)
{
colContent.SetValue(entry.Key.Q, M.Sum(M.ColVector(entry.Value)));
}
else
{
colContent.SetValue(entry.Key.Q, entry.Value[0]);
}
added = true;
}
sb.Append(colContent.ToString()); // append last column to matrix
}
sb.Append("]");
return(sb.ToString());
}
/// <summary>
/// Create the initial trajectory
/// </summary>
protected override void CreateQ0(TrajectoryInstance Q0)
{
Write(M.Assign("dist", M.Div(M.Subtract(Q.Goal(CM), Q.Start(CM)), Cfg.NSteps)));
for (int k = 1; k <= Cfg.NSteps; ++k)
{
//Q0.SetValue(Q[CM, k], Q.Start(CM) + " + " + M.Mult(k, "dist"));
}
}
/// <summary>
/// The code that calls the solver
/// </summary>
protected virtual void WriteRunFunctionBody()
{
// create & assign initial state, trajectory, bounds and linear constraints
var start = new TrajectoryStateInstance(this);
CreateStartState(start);
var goal = new TrajectoryStateInstance(this);
CreateGoalState(goal);
Write(M.Assign(Symbol.StartState, start));
Write(M.Assign(Symbol.GoalState, goal));
var Q0 = new TrajectoryInstance(this);
CreateQ0(Q0);
var lbounds = new TrajectoryInstance(this);
CreateLBounds(lbounds);
var ubounds = new TrajectoryInstance(this);
CreateUBounds(ubounds);
var Ae = new SparseTrajTensor();
var be = new List <MatlabExpression>();
CreateLinEqualityConstraints(Ae, be);
var Ai = new SparseTrajTensor();
var bi = new List <MatlabExpression>();
CreateLinEqualityConstraints(Ai, bi);
Write(M.Assign(Symbol.Q0, Q0));
Write(M.Assign(Symbol.LBounds, lbounds));
Write(M.Assign(Symbol.UBounds, ubounds));
Write(M.Assign(Symbol.AE, ToMatrix(Ae)));
Write(M.Assign(Symbol.BE, M.ColVector(be)));
Write(M.Assign(Symbol.AI, ToMatrix(Ai)));
Write(M.Assign(Symbol.BI, M.ColVector(bi)));
// set optimizer options
var optionArgs = Cfg.OptimizerOptions.SelectMany((options, idx) => options).ToArray();
Write(M.Assign(Symbol.Options, M.CallFunction(Symbol.OptimSet, optionArgs)));
// call the solver
Write(M.Assign("[Qf, fval, exitFlag, output]", M.CallFunction(Symbol.FminCon,
M.RefFunction(Symbol.CostToGoFunction), Symbol.Q0, Symbol.AI, Symbol.BI, Symbol.AE, Symbol.BE,
Symbol.LBounds, Symbol.UBounds, M.RefFunction(Symbol.NonLinConstraintFun), Symbol.Options)));
Write("output");
}
/// <summary>
/// Creates the upper bounds vector
/// </summary>
protected virtual void CreateUBounds(TrajectoryInstance bounds)
{
for (int k = 1; k <= Cfg.NSteps; ++k)
{
foreach (var q in TDef.Quantities)
{
var max = q.DefaultMax.Select(val => double.IsNaN(val) ? M.Keyword.Inf.ToString() : val.ToString()).ToArray();
bounds.SetValue(Q[q, k], max);
}
}
}
/// <summary>
/// Creates the lower bounds vector
/// </summary>
protected virtual void CreateLBounds(TrajectoryInstance bounds)
{
for (int k = 1; k <= Cfg.NSteps; ++k)
{
foreach (var q in TDef.Quantities)
{
var min = q.DefaultMin.Select(val => double.IsNaN(val) ? M.ConvertName(M.Keyword.NegInf) : val.ToString()).ToArray();
bounds.SetValue(Q[q, k], min);
}
}
}
/// <summary> /// Creates the initial state /// </summary> protected abstract void CreateQ0(TrajectoryInstance Q0);
