private void HardConstraintsStep()
{
// Vectors and matrices to store data
VectorXD C0 = new DenseVectorXD(m_numcs);
VectorXD f = new DenseVectorXD(m_numdofs);
VectorXD v = new DenseVectorXD(m_numdofs);
MatrixXD M = new DenseMatrixXD(m_numdofs, m_numdofs);
MatrixXD J = new DenseMatrixXD(m_numcs, m_numdofs);
MatrixXD System = new DenseMatrixXD(m_numcs + m_numdofs, m_numcs + m_numdofs);
VectorXD Independent = new DenseVectorXD(m_numdofs + m_numcs);
// Compute forces and retrieve the rigid bodies data
foreach (ISimulable i in m_objs)
{
i.clearForcesAndMatrices();
i.addForcesAndMatrices();
i.getForceVector(f);
i.getVelocityVector(v);
i.getMassMatrix(M);
}
// Compute and retrieve restrictions and Jacobians
foreach (Constraint c in m_constraints)
{
c.getConstraintVector(C0);
c.getConstraintJacobian(J);
}
// Set up left-side system
System.SetSubMatrix(0, m_numdofs, 0, m_numdofs, M);
System.SetSubMatrix(m_numdofs, m_numcs, 0, m_numdofs, J);
System.SetSubMatrix(0, m_numdofs, m_numdofs, m_numcs, J.Transpose());
System.SetSubMatrix(m_numdofs, m_numcs, m_numdofs, m_numcs, DenseMatrixXD.Create(m_numcs, m_numcs, 0.0));
// Set up right-side system
VectorXD b = M * v + TimeStep * f;
VectorXD AtC0 = (-1.0f / TimeStep) * C0;
Independent.SetSubVector(0, m_numdofs, b);
Independent.SetSubVector(m_numdofs, m_numcs, AtC0);
// Solve system
VectorXD newVelocities = System.Solve(Independent);
// Update bodies
foreach (ISimulable i in m_objs)
{
i.setVelocityVector(newVelocities);
i.advancePosition();
}
}
public double[] transform(double[] displacement, bool isreverse)
{
DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
double[] dispcopy = new double[6];
displacement.CopyTo(dispcopy, 0);
DenseMatrix2String m2s = new DenseMatrix2String();
List2String l2s = new List2String();
DenseVector dispV = new DenseVector(dispcopy);
log.Debug("original disp: " + l2s.ToString(displacement));
if (isreverse)
{
dispV = (DenseVector)xform.Multiply(dispV);
log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
}
DenseVector newDisp = translate(dispV.Values, isreverse);
log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
DenseMatrix rollM = roll.create(dispV.Values[3]);
DenseMatrix pitchM = pitch.create(dispV.Values[4]);
DenseMatrix yawM = yaw.create(dispV.Values[5]);
DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
log.Debug("rotation: " + m2s.ToString(rotation));
DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
log.Debug("unt: " + l2s.ToString(unt.Values));
DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
dispV.SetSubVector(0, 3, newDisp1);
if (isreverse == false)
{
dispV = (DenseVector)xform.Multiply(dispV);
}
log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
return dispV.Values;
}
public double[] transformMoments(double[] forces)
{
double[] fforce = new double[6];
forces.CopyTo(fforce, 0);
DenseVector forceV = new DenseVector(fforce);
DenseVector tforce = (DenseVector)forceV.SubVector(0, 3);
DenseVector moments = (DenseVector)forceV.SubVector(3, 3);
DenseVectorCrossProduct crs = new DenseVectorCrossProduct(tforce);
moments = (DenseVector)crs.crossProduct(directionalVector).Add(moments);
forceV.SetSubVector(3, 3, moments);
return forceV.Values;
}
public void FitQuadricThroughPoint(int line)
{
// | 0 | |x1^2-x0^2 x1-x0 x1y1-x0y0 y1-y0 y1^2-y0^2| | A |
// | | | | | B |
// | | = | | | C |
// | | | | | D |
// | 0 | |xn^2-x0^2 xn-x0 xnyn-x0y0 yn-y0 yn^2-y0^2| | E |
var points = _correctedPf[line];
Vector2 fp = points[FitPoints[line]];
Matrix<double> X = new DenseMatrix(LinePoints[line].Count - 1, 5);
for(int p = 0; p < FitPoints[line]; ++p)
{
var point = points[p];
X[p, 0] = point.X * point.X - fp.X * fp.X;
X[p, 1] = point.X - fp.X;
X[p, 2] = point.X * point.Y - fp.X * fp.Y;
X[p, 3] = point.Y - fp.Y;
X[p, 4] = point.Y * point.Y - fp.Y * fp.Y;
}
for(int p = FitPoints[line] + 1; p < LinePoints[line].Count; ++p)
{
var point = points[p];
X[p - 1, 0] = point.X * point.X - fp.X * fp.X;
X[p - 1, 1] = point.X - fp.X;
X[p - 1, 2] = point.X * point.Y - fp.X * fp.Y;
X[p - 1, 3] = point.Y - fp.Y;
X[p - 1, 4] = point.Y * point.Y - fp.Y * fp.Y;
}
var coeffs = SvdZeroFullrankSolver.Solve(X);
double F = -(coeffs[0] * fp.X * fp.X + coeffs[1] * fp.X +
coeffs[2] * fp.X * fp.Y + coeffs[3] * fp.Y + coeffs[4] * fp.Y * fp.Y);
var coeffs_f = new DenseVector(6);
coeffs_f.SetSubVector(0, 5, coeffs);
coeffs_f[5] = F;
FitQuadrics[line] = coeffs_f;
}
/*
* Layer 4: weighted Layer.
*
*/
public DenseMatrix CalculateGreatPsi(DenseMatrix X, DenseMatrix Psi)
{
int N = Psi.RowCount;
int U = Psi.ColumnCount;
int R = X.RowCount; //the number of inputs per observation
var GreatPsi = new DenseMatrix(U*(R + 1), N);
//foreach observation
for (int i = 0; i < N; i++)
{
var x = new DenseVector(R);
X.Column(i, x);
var GreatPsiCol = new DenseVector(U*(R + 1));
//foreach neuron
for (int j = 0; j < U; j++)
{
var temp = new DenseVector(x.Count + 1, 1);
temp.SetSubVector(1, x.Count, x);
GreatPsiCol.SetSubVector(j*(temp.Count), temp.Count, Psi[i, j]*temp);
}
GreatPsi.SetColumn(i, GreatPsiCol);
}
return GreatPsi;
}
private bool GomoriIteration()
{
// Шаг 1
_writer.WriteLine("Iteration: {0}", iterationNumber);
var simplexMethod = new SimplexMethod(_writer);
simplexMethod.Solve(_task); // Решаем задачу симплекс методом
_writer.WriteLine("Optimal plan is found: {0}", _task.xo);
_writer.WriteLine("Target function value = {0}", _task.c * _task.xo);
// Шаг 2
//var artJToRemoveRow = -1;
//var artJToRemoveColumn = -1;
//artJToRemoveRow = -1;
//artJToRemoveColumn = -1;
//foreach (var artJ in _artJ)
//{
// if (_task.Jb.Contains(artJ.Column))
// {
// var rowToRemove = artJ.Row; // TODO probably need to rewrite row selection
// var ai = _task.A.Row(rowToRemove); // Выбираем строку с искусственым ограничением
// ai = -ai / ai[artJ.Column];
// var rowList = ai.ToList();
// rowList.RemoveAt(artJ.Column);
// ai = DenseVector.OfEnumerable(rowList);
// var aj = _task.A.Column(artJ.Column); // Выбираем столбец с искусственным ограничением
// var columnList = aj.ToList();
// var bCoef = _task.b[rowToRemove] / columnList[rowToRemove];
// columnList.RemoveAt(rowToRemove);
// aj = DenseVector.OfEnumerable(columnList);
// var newA = DenseMatrix.Create(_task.A.RowCount - 1, _task.A.ColumnCount - 1,
// (i, j) => _task.A[i < rowToRemove ? i : i + 1, j < artJ.Column ? j : j + 1]);
// newA += DenseMatrix.OfMatrix(aj.ToColumnMatrix() * ai.ToRowMatrix()); // Удаляем искусственные строку
// _task.A = newA; // и столбец из матрицы А
// _task.b = DenseVector.Create(_task.b.Count - 1, i => i < rowToRemove ? _task.b[i] : _task.b[i + 1]);
// _task.b += bCoef * aj;
// _task.c = DenseVector.Create(_task.c.Count - 1, i => i < artJ.Column ? _task.c[i] : _task.c[i + 1]); // Удаляем искусственную переменную из вектора с
// _task.xo = DenseVector.Create(_task.xo.Count - 1, i => i < artJ.Column ? _task.xo[i] : _task.xo[i + 1]); // Удаляем искусственную переменную из xo
// _task.Jb.Remove(artJ.Column);
// artJToRemoveColumn = artJ.Column;
// artJToRemoveRow = artJ.Row;
// break;
// }
//}
//if (artJToRemoveRow > 0) // Удаляем искусственную переменную из базисных
//{
// _artJ.RemoveAll(x => x.Row == artJToRemoveRow);
// for (int i = 0; i < _artJ.Count; i++)
// {
// if (_artJ[i].Row > artJToRemoveRow)
// {
// _artJ[i].Row--; // Сдвигаем индексы базисных переменных на один
// _artJ[i].Column--; // После удаления искусственной переменной
// }
// }
// for (int i = 0; i < _task.Jb.Count; i++)
// {
// _task.Jb[i] = _task.Jb[i] > artJToRemoveColumn ? _task.Jb[i] - 1 : _task.Jb[i];
// }
//}
// Шаг 3
var falseIndex = -1;
var maxFract = 0d;
for (int i = 0; i < _task.xo.Count(); i++)
{
if (Math.Abs(Math.Round(_task.xo[i]) - _task.xo[i]) > Eps)
{
var fract = Math.Abs(_task.xo[i] - Math.Floor(_task.xo[i])); // Находим базисную переменную
if (_task.Jb.Contains(i) && fract > Eps) // С максимальной дробной частью
{ // и запоминаем ее индекс
if (fract > maxFract)
{
maxFract = fract;
falseIndex = i;
}
}
}
}
if (falseIndex < 0) // Если все переменные целые - решение найдено
{
return false; // Прерываем выполнение метода
}
_writer.WriteLine("Jk = {0}", falseIndex);
// Шаг 4
var aB = new DenseMatrix(_task.Jb.Count());
int index = 0;
foreach (var j in _task.Jb)
{
aB.SetColumn(index, _task.A.Column(j)); // Формируем матрицу Ab из базисных столбцов А
index++;
}
_writer.Write("Jb: ");
_task.Jb.ForEach(x => _writer.Write("{0} ", x));
_writer.WriteLine();
_writer.WriteLine("Basis matrix: {0}", aB);
var y = DenseMatrix.Identity(_task.A.RowCount).Column(_task.Jb.IndexOf(falseIndex)) * aB.Inverse(); //Находим e'*Ab
var newRow = new DenseVector(_task.A.ColumnCount + 1);
newRow.SetSubVector(0, _task.A.ColumnCount, y * _task.A); // Находим данные для нового отсекающего ограничения
_writer.WriteLine("Data for new limitation: {0}", newRow);
for (int i = 0; i < newRow.Count; i++) // Формируем новое отсекающее ограничение
{
if (i < _task.A.ColumnCount)
{
if (Math.Abs(newRow[i]) < Eps)
{
newRow[i] = 0;
}
else
{
newRow[i] = newRow[i] > 0
? -(newRow[i] - Math.Floor(newRow[i]))
: -(Math.Ceiling(Math.Abs(newRow[i])) - Math.Abs(newRow[i]));
}
}
else
{
newRow[i] = 1;
}
}
newRow[falseIndex] = 0;
_writer.WriteLine("New limitation: {0}", newRow);
var newb = (y * _task.b); // Находим новый элемент вектора b
newb = newb > 0 ? -(newb - Math.Floor(newb)) : -(Math.Ceiling(Math.Abs(newb)) - Math.Abs(newb)); // TODO probably need to rewrite this
_writer.WriteLine("New b = {0}", newb);
// Шаг 5
var newMatrix = new DenseMatrix(_task.A.RowCount + 1, _task.A.ColumnCount + 1); // Формируем новую
newMatrix.SetSubMatrix(0, _task.A.RowCount, 0, _task.A.ColumnCount, _task.A); // матрицу А
newMatrix.SetRow(_task.A.RowCount, newRow);
newMatrix[_task.A.RowCount, _task.A.ColumnCount] = 1;
var newBVector = new DenseVector(_task.b.Count + 1); // Формируем новый
newBVector.SetSubVector(0, _task.b.Count, _task.b); // вектор b
newBVector[_task.b.Count] = newb;
var newCVector = new DenseVector(_task.c.Count + 1); // Добавляем новую
newCVector.SetSubVector(0, _task.c.Count, _task.c); // компоненту вектора с
var newJb = _task.Jb.ToList();
newJb.Add(newJb[newJb.Count - 1] + 1);
_artJ.Add(new ArtJEntry { Column = newMatrix.ColumnCount - 1, Row = newMatrix.RowCount - 1 });
_task.A = newMatrix.Clone(); // Создаем
_task.b = newBVector.Clone(); // новую задачу
_task.c = newCVector.Clone(); // для следующей итерации
_task.Jb = newJb;
iterationNumber++; // Присваиваем новый номер итерации
return true;
}
private void InsertNewRestrictionRow(DenseVector insF, double insB)
{
CurrA = (DenseMatrix) CurrA.InsertRow(CurrA.RowCount, -insF); // TODO: test this
var newColumn = new DenseVector(CurrA.RowCount);
newColumn[CurrA.RowCount - 1] = 1;
CurrA = (DenseMatrix) CurrA.InsertColumn(CurrA.ColumnCount, newColumn);
var newB = new DenseVector(CurrB.Count + 1);
newB.SetSubVector(0, CurrB.Count, CurrB);
newB[CurrB.Count] = -insB;
CurrB = newB;
var newC = new DenseVector(CurrC.Count + 1);
newC.SetSubVector(0, CurrC.Count, CurrC);
CurrC = newC;
}
private void HardConstraintsStep()
{
// Apuntes miki + apuntes clase
// Step de Fuertes:
// Tenemos que conseguir una matriz enorme formada por
//
// | A J^t |
// | J 0 |
// A es la matriz de masas-> 6 * cada componente como en soft
// J es la matriz jacobiana que rellenamos en Constraint.getConstraintJacobian
// J se forma 3 * numero de constraints, 6* num rigidbodies
// J^t se forma 6* num rigidbodies, 3 * numero de constraints
// Recordemos que cada rigidbody tiene 6 numdof
// C0 = 3 * numero de constraints<- Es similar a C en soft
MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6);
MatrixXD jacobian = new DenseMatrixXD(3 * Constraints.Count, 6 * m_objs.Count);
VectorXD C0 = new DenseVectorXD(3 * m_constraints.Count);
// paso 1: necesitamos settear la jacobiana y C0 para cada restricción
foreach (Constraint c in m_constraints)
{
c.getConstraintVector(C0);
c.getConstraintJacobian(jacobian);
}
// las fuerzas son 1 x 6 * m_objs.Count, igual que las velocidades.
// Parece que se mete el torque y demás. 3 * objetos no funciona
VectorXD total_force = new DenseVectorXD(6 * m_objs.Count);
VectorXD total_velocities = new DenseVectorXD(6 * m_objs.Count);
// PARA CADA RIGIDBODY
// Paso 2: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error
// Paso 3: Establecemos la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs
// Paso 4: Metemos valores en las velocidades anteriores, ¿Por qué? Porque Al final haremos un A * v = b
// como en las prácticas anteriores y b es un vector formado por b y -1 * C0 / TimeStep
// siendo b la fórmula (matriz_de_masas * velocidades) + (TimeStep * fuerzas);
// Paso 5: Añadimos fuerzas y matrices (que las hemos limpiado antes)
// Paso 6: Metemos las nuevas fuerzas (getForceVector) en el vector de fuerzas
foreach (RigidBody obj in m_objs)
{
obj.clearForcesAndMatrices();
obj.getMassMatrix(massMatrix);
obj.getVelocityVector(total_velocities);
obj.addForcesAndMatrices();
obj.getForceVector(total_force);
}
// Creamos la super matriz que hemos dicho con la de masas, jacobianas y ceros
// Al crear una MtrixXD se crea con ceros.
MatrixXD jacobianT = jacobian.Transpose();
MatrixXD ceroMatrix = new DenseMatrixXD(3 * m_constraints.Count, 3 * m_constraints.Count);
DenseMatrixXD megaMatrix = new DenseMatrixXD(jacobian.RowCount + massMatrix.RowCount, jacobianT.ColumnCount + massMatrix.ColumnCount);
// la matriz de masas es 0,0 a (m_objs.Count * 6)
// Una vez acaba esa, va la jacobiana traspuesta
// Debajo de la misma manera va la jacobiana
// y en la esquina abajo derecha va la de ceros
megaMatrix.SetSubMatrix(0, 0, massMatrix);
megaMatrix.SetSubMatrix(0, 0 + massMatrix.ColumnCount, jacobianT);
megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0, jacobian);
megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0 + massMatrix.ColumnCount, ceroMatrix);
// M * v = M * v0 + timestep * fuerzas
// V0 es la velocidad del step anterior
// en A * v = b, b lo dividiremos como b y b2
// y los concatenamos
VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force);
VectorXD b2 = -1 * C0 / TimeStep;
// b total - ambos bs
VectorXD realB = new DenseVectorXD(b.Count + b2.Count);
realB.SetSubVector(0, b.Count, b);
realB.SetSubVector(b.Count, b2.Count, b2);
// V se forma por velocidades y un vector de lamdas, que debe ser del tamaño de C0 ya que
// b2 es escalar * c0
VectorXD lamdas = new DenseVectorXD(C0.Count);
// conjunto de velocidades formada por las velocidades y las lamdas
VectorXD megaV = new DenseVectorXD(total_velocities.Count + lamdas.Count);
megaV.SetSubVector(0, total_velocities.Count, total_velocities);
megaV.SetSubVector(total_velocities.Count, lamdas.Count, lamdas);
// Resolvemos el sistema
megaV = megaMatrix.Solve(realB);
// nueva velocidad
VectorXD newVelocities = megaV.SubVector(0, total_velocities.Count);
// Establecemos las nuevas posiciones y velocidades
foreach (RigidBody obj in m_objs)
{
obj.setVelocityVector(newVelocities);
obj.advancePosition();
}
}
public double[] calcNewDiffs(double[] cartesian)
{
DenseMatrix rotTrig = DenseMatrix.Create(3,2,0);
double theta_x = cartesian[3];
double theta_y = cartesian[4];
double theta_z = cartesian[5];
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
rotTrig.At(0, 0, Math.Sin(theta_x));
rotTrig.At(0, 1, Math.Cos(theta_x));
rotTrig.At(1, 0, Math.Sin(theta_y));
rotTrig.At(1, 1, Math.Cos(theta_y));
rotTrig.At(2, 0, Math.Sin(theta_z));
rotTrig.At(2, 1, Math.Cos(theta_z));
log.Debug("Rotation Angles " + rotTrig.ToString("#000.00000\t", formatProvider));
DenseVector basepin = fixedPin;
DenseVector nominalplatformpin = initialPlatformPin;
double Ppx = nominalplatformpin[0];
double Ppy = nominalplatformpin[1];
double Ppz = nominalplatformpin[2];
log.Debug("Ppx= " + Ppx + "Ppy= " + Ppy + "Ppz= " + Ppz);
DenseVector currentplatformpin = platformPin;
List2String l2s = new List2String();
DenseVector phai = (DenseVector) currentplatformpin.Subtract(basepin);
log.Debug("phai " + l2s.ToString(phai.Values));
if (length == 0.0)
{
log.Error("Actuator length is zero. Caused a divide-by-zero error");
throw new Exception("Actuator length is zero causing a divide by zero error");
}
phai = (DenseVector) phai.Divide(length);
log.Debug("phai " + l2s.ToString(phai.Values));
DenseMatrix J = DenseMatrix.Create(3,3,0);
// Jacobian Element
J[0, 0] = 0;
J[0, 1] = -rotTrig[1, 0] * rotTrig[2, 1] * Ppx + rotTrig[1, 0] * rotTrig[2, 0] * Ppy + rotTrig[1, 1] * Ppz;
J[0, 2] = -rotTrig[1, 1] * rotTrig[2, 0] * Ppx - rotTrig[1, 1] * rotTrig[2, 1] * Ppy;
J[1, 0] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppx +
(-rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] - rotTrig[0, 0] * rotTrig[0, 1]) * Ppy
- rotTrig[0, 1] * rotTrig[1, 1] * Ppz;
J[1, 1] = rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx - rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
- rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
J[1, 2] = (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] * rotTrig[0, 1] * rotTrig[2, 1]) * Ppx +
(-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 1] * rotTrig[2, 0]) * Ppy;
J[2, 0] = (rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] + rotTrig[0, 1] * rotTrig[2, 0]) * Ppx +
(-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 1] * rotTrig[2, 1]) * Ppy
- rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
J[2, 1] = -rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx + rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
- rotTrig[0, 1] * rotTrig[1, 0] * Ppz;
J[2, 2] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 0] * rotTrig[2, 1]) * Ppx
+ (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppy;
log.Debug("Jacobian " + J.ToString("#000.00000\t", formatProvider));
DenseVector diffs = new DenseVector(6);
diffs.SetSubVector(0, 3, phai);
DenseVector rphai = new DenseVector(new[] { (J[0, 0] * phai[0] + J[1, 0] * phai[1] + J[2, 0] * phai[2]),
(J[0, 1] * phai[0] + J[1, 1] * phai[1] + J[2, 1] * phai[2]),
(J[0, 2] * phai[0] + J[1, 2] * phai[2] + J[2, 2] * phai[2]) });
log.Debug("rphai " + l2s.ToString(rphai.Values));
diffs.SetSubVector(3, 3, rphai);
return diffs.Values;
}