/// <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>
/// Create the goal state
/// </summary>
protected override void CreateGoalState(TrajectoryStateInstance goal)
{
goal.SetValue(CM, Settings.GoalCM);
goal.SetValue(d, Settings.GoalD);
}
/// <summary>
/// Create the initial state
/// </summary>
protected override void CreateStartState(TrajectoryStateInstance start)
{
start.SetValue(CM, Settings.StartCM);
}
/// <summary> /// Creates the initial state /// </summary> protected abstract void CreateGoalState(TrajectoryStateInstance goal);
/// <summary> /// Creates the initial state /// </summary> protected abstract void CreateStartState(TrajectoryStateInstance start);
