/// <summary>
/// Add a constraint
/// </summary>
private Constraint AddConstraint(List <MatlabExpression> constraints, MatlabExpression expr, int nDims)
{
constraints.Add(expr);
return(new Constraint {
Index = constraints.Count - 1, NDims = nDims
});
}
public void SetValue(TrajectoryQuantityInstance q, MatlabExpression value)
{
var idx = q.GlobalIndex;
Debug.Assert(idx >= 0 && idx < Values.Length);
Values[idx] = value;
}
public void Write(MatlabExpression expr)
{
ExecuteSafely(() =>
{
expr.Write(this);
WriteLine();
});
}
public void SetValue(TrajectoryQuantityInstance q, MatlabExpression value)
{
var idx = q.GlobalIndex;
if (idx >= 0 && idx < Values.Length)
{
Values[idx] = value;
}
// Silently ignore invalid values
}
public static MatlabExpression If(MatlabExpression condition, params MatlabExpression[] code)
{
var n = 2 + code.Length;
var arr = new MatlabExpression[n];
arr[0] = If(condition);
for (var i = 1; i < n - 1; ++i)
{
arr[i] = code[i - 1];
}
arr[n - 1] = End();
return(new MatlabStatementList(arr));
}
public void Add(Idx idx, MatlabExpression item)
{
List <MatlabExpression> list;
if (!TryGetValue(idx, out list))
{
Add(idx, list = new List <MatlabExpression>());
}
else
{
ToString();
}
list.Add(item);
}
protected void AddCostGrad(TrajectoryQuantityInstance q, MatlabExpression expr)
{
var idx = q.GlobalIndex;
if (idx >= 0 && idx < JGradCode.Length)
{
var list = JGradCode[idx];
if (list == null)
{
JGradCode[idx] = list = new List <MatlabExpression>(5);
}
list.Add(expr);
}
// for now: Silently ignore invalid gradients (such as those from the boundaries)
}
public static MatlabExpression If(MatlabExpression condition)
{
return(new MatlabStatement("if " + condition, MatlabStatementFlags.Opening));
}
/// <summary>
/// Gradient of: norm(to - from) with respect to "to"
/// </summary>
public static MatlabExpression NormGrad(MatlabExpression vec)
{
return(new MatlabExpression(string.Format("{0}/norm({0})", vec)));
}
/// <summary>
/// Gradient of: norm(to - from) with respect to "to"
/// </summary>
public static MatlabExpression NormGrad(MatlabExpression to, MatlabExpression from)
{
return(new MatlabExpression(string.Format("({0} - {1})/norm({0} - {1})", to, from)));
}
public static MatlabExpression Sum(MatlabExpression expr)
{
return(new MatlabExpression(string.Format("sum([{0}])", expr)));
}
public static MatlabExpression Transpose(MatlabExpression expr)
{
return(new MatlabExpression(string.Format("({0})'", expr)));
}
/// <summary>
/// Add a constraint gradient
/// </summary>
private void AddConstraintGrad(SparseMatlabTensor <QInstanceIndex> constraintGrad, TrajectoryQuantityInstance q, Constraint ci, MatlabExpression expr)
{
constraintGrad.Add(new QInstanceIndex(ci, q), expr);
}
/// <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>
/// Add an equality constraint gradient
/// </summary>
protected void AddIConstraintGrad(Constraint ci, TrajectoryQuantityInstance q, MatlabExpression expr)
{
AddConstraintGrad(IConstraintsNLGradCode, q, ci, expr);
}
/// <summary>
/// Add a multi-dimensional equality constraint gradient
/// </summary>
protected void AddEConstraintGrad(Constraint ci, TrajectoryQuantityInstance q, MatlabExpression expr)
{
if (expr.ToString() == "- 1*1")
{
Console.WriteLine("{0}, {1}: {2}", ci.Index, q, expr);
}
AddConstraintGrad(EConstraintsNLGradCode, q, ci, expr);
//if (ci.NDims > 1)
}
protected Constraint AddIConstraint(MatlabExpression expr, int nDims = 1)
{
return(AddConstraint(IConstraintsNLCode, expr, nDims));
}
/// <summary>
/// Add a dynamics equality constraint of the form qNew - qOld - h qDot = 0
/// </summary>
protected Constraint AddDynamicsEConstraint(object qNew, object qOld, MatlabExpression qDot, int nDims = 1)
{
return(AddEConstraint(string.Format("{0} - {1} - {2} * ({3})", qNew, qOld, Cfg.H, qDot), nDims));
}
public static MatlabExpression CellArray(MatlabExpression expr)
{
return("{" + expr + "}");
}
protected void AddCost(MatlabExpression expr)
{
JCode.Add(expr);
}
