/// <summary>
/// Add a quantity
/// </summary>
public void AddQuantity(TrajectoryQuantity q)
{
var lastIdx = Quantities.Count;
var nOccurancesDelta = q.IsTransition ? 0 : -1;
TotalTrajectorySize += (Program.Cfg.NSteps + nOccurancesDelta) * q.Length;
SingleStepSize += q.Length;
q.Index = lastIdx;
if (lastIdx > 0)
{
q.Offset = Quantities.Last().Offset + Quantities.Last().Length;
}
else
{
q.Offset = 1;
}
if (lastIdx > 0 && Quantities.Last().IsTransition)
{
// Transitions must be added to the end
Debug.Assert(q.IsTransition);
}
else if (q.IsTransition)
{
// The first transition
StateQuantityCount = lastIdx;
StateQuantitySize = q.Offset - 1;
}
Quantities.Add(q);
}
/// <summary>
/// Returns "Q(idx)", where Q is this var's name and idx is the index set of the given StateQuantity in the k'th component.
/// </summary>
public NamedTrajectoryQuantityInstance this[TrajectoryQuantity sq, int k]
{
get
{
return(Instances[sq.GetGlobalIndexWBoundaries(k)]);
}
}
public TrajectoryQuantity AddQuantity(TrajectoryQuantityType type, double[] min, double[] max)
{
var q = new TrajectoryQuantity(Program, type, min, max);
AddQuantity(q);
return(q);
}
public void SetValue <T>(TrajectoryQuantity q, params T[] exprs)
{
Debug.Assert(q.Length == exprs.Count());
var idx = q.Index;
Debug.Assert(idx >= 0 && idx < Values.Length);
Values[idx] = M.ColVector(exprs);
}
/// <summary>
/// Add a dynamics equality constraint of the form q(k+1) - q(k) - h qDot(k+1) = 0
/// </summary>
protected Constraint AddVelocityIntegrationEConstraintAndGradient(int k, TrajectoryQuantity q, TrajectoryQuantity qDot)
{
Debug.Assert(q.Length == qDot.Length);
// qNew - qOld - h qDot = 0
var qNew = Q[q, k + 1]; var qOld = Q[q, k];
var qDotK = Q[qDot, k];
var ci = AddDynamicsEConstraint(qNew, qOld, qDotK, q.Length);
AddEConstraintGrad(ci, qNew, M.Eye(q.Length));
AddEConstraintGrad(ci, qOld, "-" + M.Eye(q.Length));
AddEConstraintGrad(ci, qDotK, "- " + Cfg.H + "*" + M.Eye(q.Length));
return(ci);
}
/// <summary>
/// Add a dynamics equality constraint of the form:
/// qNew - qOld - h qDot = 0
/// qDotNew - qDotOld - h qDDot = 0
/// Also adds all standard gradients (all but the gradients of qDDot).
/// </summary>
/// <returns>The force constraint, so the gradients of qDDot can be added.</returns>
protected Constraint AddDynamicsEConstraintAndGradients(int k, TrajectoryQuantity q, TrajectoryQuantity qDot, MatlabExpression qDDot)
{
Debug.Assert(q.Length == qDot.Length);
// qNew - qOld - h qDot = 0
var qDotNew = Q[qDot, k + 1]; var qDotOld = Q[qDot, k];
var c1 = AddVelocityIntegrationEConstraintAndGradient(k, q, qDot);
// qDotNew - qDotOld - h qDDot = 0
var c2 = AddDynamicsEConstraint(qDotNew, qDotOld, qDDot, q.Length);
AddEConstraintGrad(c2, qDotNew, M.Eye(qDot.Length));
AddEConstraintGrad(c2, qDotOld, "-" + M.Eye(qDot.Length));
return(c2);
}
/// <summary>
/// Use above quantities to define trajectory
/// </summary>
protected override void AddQuantities()
{
var inf = double.NaN;
var inf2 = new[] { inf, inf };
CM = TDef.AddQuantity(TrajectoryQuantityType.State, Settings.BBoxMin, Settings.BBoxMax);
CMDot = TDef.AddQuantity(TrajectoryQuantityType.Velocity, inf2, inf2);
//theta = TDef.AddQuantity(TrajectoryQuantityType.State, inf, inf);
//thetaDot = TDef.AddQuantity(TrajectoryQuantityType.Velocity, inf, inf);
d = TDef.AddQuantity(TrajectoryQuantityType.State, -Settings.DMax, Settings.DMax);
dDot = TDef.AddQuantity(TrajectoryQuantityType.Velocity, inf, inf);
lambda1 = TDef.AddQuantity(TrajectoryQuantityType.Lambda, 0, inf);
//lambda2 = TDef.AddQuantity(TrajectoryQuantityType.Lambda, 0, inf);
//uTheta = TDef.AddQuantity(TrajectoryQuantityType.Actuation, -Settings.UThetaMax, Settings.UThetaMax);
uD = TDef.AddQuantity(TrajectoryQuantityType.Actuation, -Settings.UDMax, Settings.UDMax);
}
/// <summary>
/// Goal state & transition
/// </summary>
public NamedTrajectoryQuantityInstance Goal(TrajectoryQuantity q)
{
return(this[q, Program.Cfg.NSteps + 1]);
}
/// <summary>
/// Start state & transition
/// </summary>
public NamedTrajectoryQuantityInstance Start(TrajectoryQuantity q)
{
return(this[q, 1]);
}
/// <summary>
/// The index of the first scalar of the given quantity in the problem trajectory (exlcuding boundaries)
/// </summary>
public int GetGlobalOffset(TrajectoryQuantity q, int k)
{
return(SingleStepSize * (k - 1) + q.Offset - StateQuantitySize);
}
/// <summary>
/// The index of the given quantity in the problem trajectory (including boundaries)
/// </summary>
public int GetGlobalIndexWBoundaries(TrajectoryQuantity q, int k)
{
return(SingleStepQuantityCount * (k - 1) + q.Index);
}
/// <summary>
/// The index of the given quantity in the problem trajectory (exlcuding boundaries)
/// </summary>
public int GetGlobalIndex(TrajectoryQuantity q, int k)
{
return(GetGlobalIndexWBoundaries(q, k) - InitialBoundaryLength);
}
public TrajectoryQuantityInstance(int k, TrajectoryQuantity quantity)
{
K = k;
Quantity = quantity;
Value = this;
}
public NamedTrajectoryQuantityInstance(TrajectoryVar var, int k, TrajectoryQuantity quantity)
: base(k, quantity)
{
Var = var;
}