private void Calculate2ndDerivatives(Vector tau)
{
double cos1 = Math.Cos(Arc1);
double cos12 = Math.Cos(Arc1 + Arc2);
double cos123 = Math.Cos(Arc1 + Arc2 + Arc3);
double cos2 = Math.Cos(Arc2);
double cos23 = Math.Cos(Arc2 + Arc3);
double cos3 = Math.Cos(Arc3);
double sin2 = Math.Sin(Arc2);
double sin23 = Math.Sin(Arc2 + Arc3);
double sin3 = Math.Sin(Arc3);
b[0, 0] = 3 + 2 * cos2 + m * (3 + 2 * cos2 + 2 * cos23 + 2 * cos3);
b[0, 1] = 1 + cos2 + m * (2 + cos2 + cos23 + 2 * cos3);
b[0, 2] = m * (1 + cos23 + cos3);
b[1, 0] = 1 + cos2 + m * (2 + cos2 + cos23 + 2 * cos3);
b[1, 1] = 1 + m * (2 + 2 * cos3);
b[1, 2] = m * (1 + cos3);
b[2, 0] = m * (1 + cos23 + cos3);
b[2, 1] = m * (1 + cos3);
b[2, 2] = m;
h[0] = g * ((2 + m) * cos1 + (1 + m) * cos12 + m * cos123);
h[1] = g * ((1 + m) * cos12 + m * cos123);
h[2] = g * m * cos123;
c[0] = 0
+ dArc[0] * dArc[1] * (-2 * sin2 - m * (2 * sin2 + 2 * sin23))
+ dArc[0] * dArc[2] * -m * (2 * sin23 + 2 * sin3)
+ dArc[1] * dArc[1] * (-sin2 - m * (sin2 + sin23))
+ dArc[1] * dArc[2] * -m * (2 * sin23 + 2 * sin3)
+ dArc[2] * dArc[2] * -m * (sin23 + sin3);
c[1] = dArc[0] * dArc[0] * (sin2 + m * (sin2 + sin23))
+ 0
+ dArc[0] * dArc[2] * -m * 2 * sin3
+ 0
+ dArc[1] * dArc[2] * -m * 2 * sin3
+ dArc[2] * dArc[2] * -m * sin3;
c[2] = dArc[0] * dArc[0] * m * (sin23 + sin3)
+ dArc[0] * dArc[1] * m * 2 * sin3
+ 0
+ dArc[1] * dArc[1] * m * sin3
+ 0
+ 0;
// TODO: determine what exceptions Solve can throw, or how does it signal calculation failure
////try
////{
LU lu = new DenseLU(b);
d2Arc = lu.Solve(tau - c - h);
////}
////catch
////{
//// d2Arc.Clear();
////}
}
public override Core.Reinforcement PerformAction(Core.Action <double> action)
{
double force = Math.Max(minF, Math.Min(action[0], maxF));
double reward = -forcePenalty *Math.Abs(action[0] - force);
Vector <double> ddq = null;
for (double t = 0; t < externalDiscretization; t += internalDiscretization)
{
CalculateBChf(force);
for (int i = 4; i < 8; ++i)
{
dq.At(i - 4, CurrentState[i]);
}
LU lu = new DenseLU(b);
ddq = lu.Solve(f - h - c * dq);
double dt = Math.Min(internalDiscretization, externalDiscretization - t);
double dt2 = dt * dt;
for (int i = 0; i < 4; i++)
{
CurrentState[i] += dt * CurrentState[4 + i] + 0.5 * dt2 * ddq[i];
CurrentState[4 + i] += dt * ddq[i];
}
}
for (int i = 1; i < 4; i++)
{
if (CurrentState[i] > Math.PI)
{
CurrentState[i] -= Math.PI * 2;
}
if (CurrentState[i] < -Math.PI)
{
CurrentState[i] += Math.PI * 2;
}
}
CurrentState.IsTerminal = !IsStateOK();
if (IsStateOK())
{
reward -= Math.Abs(CurrentState[0]);
reward -= Math.Abs(CurrentState[1]);
reward -= Math.Abs(CurrentState[2]);
reward -= Math.Abs(CurrentState[3]);
}
else
{
reward -= boundryPenalty;
}
return(reward);
}
/// <summary>
/// Solves the linear system Ax=b.
/// </summary>
/// <param name="input">The right hand side vector.</param>
/// <returns></returns>
public static double[] Solve(this DenseColumnMajorStorage <double> matrix, double[] input)
{
int columns = matrix.ColumnCount;
var lu = DenseLU.Create(matrix);
var x = new double[columns];
lu.Solve(input, x);
return(x);
}
/// <summary>
/// Gets the inverse of the matrix.
/// </summary>
/// <returns></returns>
public static Matrix Inverse(this DenseColumnMajorStorage <double> matrix)
{
int rows = matrix.RowCount;
int columns = matrix.ColumnCount;
var inv = new Matrix(rows, columns);
DenseLU.Create(matrix).Inverse(inv);
return(inv);
}
/// <summary>
/// Gets the determinant of the matrix.
/// </summary>
/// <returns></returns>
public static double Determinant(this DenseColumnMajorStorage <double> matrix)
{
return(DenseLU.Create(matrix).Determinant());
}
public override LU <double> LU()
{
return(DenseLU.Create(this));
}
public override LU <float> LU()
{
return(DenseLU.Create(this));
}
public override LU <Complex32> LU()
{
return(DenseLU.Create(this));
}
