// Helper routine for SVD conversion from bidiagonal to diagonal
bool MatrixRmn::UpdateBidiagIndices(long *firstBidiagIdx, long *lastBidiagIdx, VectorRn &w, VectorRn &superDiag, double eps)
{
long lastIdx = *lastBidiagIdx;
double *sdPtr = superDiag.GetPtr(lastIdx - 1); // Entry above the last diagonal entry
while (NearZero(*sdPtr, eps)) {
*(sdPtr--) = 0.0;
lastIdx--;
if (lastIdx == 0) {
return false;
}
}
*lastBidiagIdx = lastIdx;
long firstIdx = lastIdx - 1;
double *wPtr = w.GetPtr(firstIdx);
while (firstIdx > 0) {
if (NearZero(*wPtr, eps)) { // If this diagonal entry (near) zero
*wPtr = 0.0;
break;
}
if (NearZero(*(--sdPtr), eps)) { // If the entry above the diagonal entry is (near) zero
*sdPtr = 0.0;
break;
}
wPtr--;
firstIdx--;
}
*firstBidiagIdx = firstIdx;
return true;
}
// ********************************************************************************************
// Singular value decomposition.
// Return othogonal matrices U and V and diagonal matrix with diagonal w such that
// (this) = U * Diag(w) * V^T (V^T is V-transpose.)
// Diagonal entries have all non-zero entries before all zero entries, but are not
// necessarily sorted. (Someday, I will write ComputedSortedSVD that handles
// sorting the eigenvalues by magnitude.)
// ********************************************************************************************
void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
{
assert ( U.NumRows==NumRows && V.NumCols==NumCols
&& U.NumRows==U.NumCols && V.NumRows==V.NumCols
&& w.GetLength()==Min(NumRows,NumCols) );
// double temp=0.0;
VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around.
// Choose larger of U, V to hold intermediate results
// If U is larger than V, use U to store intermediate results
// Otherwise use V. In the latter case, we form the SVD of A transpose,
// (which is essentially identical to the SVD of A).
MatrixRmn* leftMatrix;
MatrixRmn* rightMatrix;
if ( NumRows >= NumCols ) {
U.LoadAsSubmatrix( *this ); // Copy A into U
leftMatrix = &U;
rightMatrix = &V;
}
else {
V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V
leftMatrix = &V;
rightMatrix = &U;
}
// Do the actual work to calculate the SVD
// Now matrix has at least as many rows as columns
CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );
}
// Multiply transpose of this matrix by column vector v.
// Result is column vector "result"
// Equivalent to mult by row vector on left
void MatrixRmn::MultiplyTranspose(const VectorRn &v, VectorRn &result) const
{
assert(v.GetLength() == NumRows && result.GetLength() == NumCols);
double *out = result.GetPtr(); // Points to entry in result vector
const double *colPtr = x; // Points to beginning of next column in matrix
for (long i = NumCols; i > 0; i--) {
const double *in = v.GetPtr();
*out = 0.0f;
for (long j = NumRows; j > 0; j--) {
*out += (*(in++)) * (*(colPtr++));
}
out++;
}
}
bool MatrixRmn::DebugCheckSVD(const MatrixRmn &U, const VectorRn &w, const MatrixRmn &V) const
{
// Special SVD test code
MatrixRmn IV(V.getNumRows(), V.getNumColumns());
IV.SetIdentity();
MatrixRmn VTV(V.getNumRows(), V.getNumColumns());
MatrixRmn::TransposeMultiply(V, V, VTV);
IV -= VTV;
double error = IV.FrobeniusNorm();
MatrixRmn IU(U.getNumRows(), U.getNumColumns());
IU.SetIdentity();
MatrixRmn UTU(U.getNumRows(), U.getNumColumns());
MatrixRmn::TransposeMultiply(U, U, UTU);
IU -= UTU;
error += IU.FrobeniusNorm();
MatrixRmn Diag(U.getNumRows(), V.getNumRows());
Diag.SetZero();
Diag.SetDiagonalEntries(w);
MatrixRmn B(U.getNumRows(), V.getNumRows());
MatrixRmn C(U.getNumRows(), V.getNumRows());
MatrixRmn::Multiply(U, Diag, B);
MatrixRmn::MultiplyTranspose(B, V, C);
C -= *this;
error += C.FrobeniusNorm();
bool ret = (fabs(error) <= 1.0e-13 * w.MaxAbs());
assert(ret);
return ret;
}
// Solves the equation (*this)*xVec = b;
// Uses row operations. Assumes *this is square and invertible.
// No error checking for divide by zero or instability (except with asserts)
void MatrixRmn::Solve(const VectorRn &b, VectorRn *xVec) const
{
assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
// Copy this matrix and b into an Augmented Matrix
MatrixRmn &AugMat = GetWorkMatrix(NumRows, NumCols + 1);
AugMat.LoadAsSubmatrix(*this);
AugMat.SetColumn(NumRows, b);
// Put into row echelon form with row operations
AugMat.ConvertToRefNoFree();
// Solve for x vector values using back substitution
double *xLast = xVec->x + NumRows - 1; // Last entry in xVec
double *endRow = AugMat.x + NumRows * NumCols - 1; // Last entry in the current row of the coefficient part of Augmented Matrix
double *bPtr = endRow + NumRows; // Last entry in augmented matrix (end of last column, in augmented part)
for (long i = NumRows; i > 0; i--) {
double accum = *(bPtr--);
// Next loop computes back substitution terms
double *rowPtr = endRow; // Points to entries of the current row for back substitution.
double *xPtr = xLast; // Points to entries in the x vector (also for back substitution)
for (long j = NumRows - i; j > 0; j--) {
accum -= (*rowPtr) * (*(xPtr--));
rowPtr -= NumCols; // Previous entry in the row
}
assert(*rowPtr != 0.0); // Are not supposed to be any free variables in this matrix
*xPtr = accum / (*rowPtr);
endRow--;
}
}
// Multiply this matrix by column vector v.
// Result is column vector "result"
void MatrixRmn::Multiply(const VectorRn &v, VectorRn &result) const
{
assert(v.GetLength() == NumCols && result.GetLength() == NumRows);
double *out = result.GetPtr(); // Points to entry in result vector
const double *rowPtr = x; // Points to beginning of next row in matrix
for (long j = NumRows; j > 0; j--) {
const double *in = v.GetPtr();
const double *m = rowPtr++;
*out = 0.0f;
for (long i = NumCols; i > 0; i--) {
*out += (*(in++)) * (*m);
m += NumRows;
}
out++;
}
}
// Set the i-th column equal to d.
void MatrixRmn::SetColumn(long i, const VectorRn &d)
{
assert(NumRows == d.GetLength());
double *to = x + i * NumRows;
const double *from = d.x;
for (i = NumRows; i > 0; i--) {
*(to++) = *(from++);
}
}
// Set the i-th column equal to d.
void MatrixRmn::SetRow(long i, const VectorRn &d)
{
assert(NumCols == d.GetLength());
double *to = x + i;
const double *from = d.x;
for (i = NumRows; i > 0; i--) {
*to = *(from++);
to += NumRows;
}
}
// Form the dot product of a vector v with the i-th column of the array
double MatrixRmn::DotProductColumn(const VectorRn &v, long colNum) const
{
assert(v.GetLength() == NumRows);
double *ptrC = x + colNum * NumRows;
double *ptrV = v.x;
double ret = 0.0;
for (long i = NumRows; i > 0; i--) {
ret += (*(ptrC++)) * (*(ptrV++));
}
return ret;
}
bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
{
// Special SVD test code
MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
IV.SetIdentity();
MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
MatrixRmn::TransposeMultiply( V, V, VTV );
IV -= VTV;
double error = IV.FrobeniusNorm();
MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
IU.SetIdentity();
MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
MatrixRmn::TransposeMultiply( U, U, UTU );
IU -= UTU;
error += IU.FrobeniusNorm();
MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
DiagAndSuper.SetZero();
DiagAndSuper.SetDiagonalEntries( w );
if ( this->GetNumRows()>=this->GetNumColumns() ) {
DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
}
else {
DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
}
MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
MatrixRmn::Multiply( U, DiagAndSuper, B );
MatrixRmn::MultiplyTranspose( B, V, C );
C -= *this;
error += C.FrobeniusNorm();
bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
assert ( ret );
return ret;
}
// **************** ConvertBidiagToDiagonal ***********************************************
// Do the iterative transformation from bidiagonal form to diagonal form using
// Givens transformation. (Golub-Reinsch)
// U and V are square. Size of U less than or equal to that of U.
void MatrixRmn::ConvertBidiagToDiagonal(MatrixRmn &U, MatrixRmn &V, VectorRn &w, VectorRn &superDiag) const
{
// These two index into the last bidiagonal block (last in the matrix, it will be
// first one handled.
long lastBidiagIdx = V.NumRows - 1;
long firstBidiagIdx = 0;
//togliere
double aa = w.MaxAbs();
double bb = superDiag.MaxAbs();
double eps = 1.0e-15 * std::max(w.MaxAbs(), superDiag.MaxAbs());
while (true) {
bool workLeft = UpdateBidiagIndices(&firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps);
if (!workLeft) {
break;
}
// Get ready for first Givens rotation
// Push non-zero to M[2,1] with Givens transformation
double *wPtr = w.x + firstBidiagIdx;
double *sdPtr = superDiag.x + firstBidiagIdx;
double extraOffDiag = 0.0;
if ((*wPtr) == 0.0) {
ClearRowWithDiagonalZero(firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps);
if (firstBidiagIdx > 0) {
if (NearZero(*(--sdPtr), eps)) {
*sdPtr = 0.0;
} else {
ClearColumnWithDiagonalZero(firstBidiagIdx, V, wPtr, sdPtr, eps);
}
}
continue;
}
// Estimate an eigenvalue from bottom four entries of M
// This gives a lambda value which will shift the Givens rotations
// Last four entries of M^T * M are ( ( A, B ), ( B, C ) ).
double A;
A = (firstBidiagIdx < lastBidiagIdx - 1) ? Square(superDiag[lastBidiagIdx - 2]) : 0.0;
double BSq = Square(w[lastBidiagIdx - 1]);
A += BSq; // The "A" entry of M^T * M
double C = Square(superDiag[lastBidiagIdx - 1]);
BSq *= C; // The squared "B" entry
C += Square(w[lastBidiagIdx]); // The "C" entry
double lambda; // lambda will hold the estimated eigenvalue
lambda = sqrt(Square((A - C) * 0.5) + BSq); // Use the lambda value that is closest to C.
if (A > C) {
lambda = -lambda;
}
lambda += (A + C) * 0.5; // Now lambda equals the estimate for the last eigenvalue
double t11 = Square(w[firstBidiagIdx]);
double t12 = w[firstBidiagIdx] * superDiag[firstBidiagIdx];
double c, s;
CalcGivensValues(t11 - lambda, t12, &c, &s);
ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, wPtr + 1);
V.PostApplyGivens(c, -s, firstBidiagIdx);
long i;
for (i = firstBidiagIdx; i < lastBidiagIdx - 1; i++) {
// Push non-zero from M[i+1,i] to M[i,i+2]
CalcGivensValues(*wPtr, extraOffDiag, &c, &s);
ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr + 1, sdPtr + 1);
U.PostApplyGivens(c, -s, i);
// Push non-zero from M[i,i+2] to M[1+2,i+1]
CalcGivensValues(*sdPtr, extraOffDiag, &c, &s);
ApplyGivensCBTD(c, s, sdPtr, wPtr + 1, &extraOffDiag, extraOffDiag, sdPtr + 1, wPtr + 2);
V.PostApplyGivens(c, -s, i + 1);
wPtr++;
sdPtr++;
}
// Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
CalcGivensValues(*wPtr, extraOffDiag, &c, &s);
ApplyGivensCBTD(c, s, wPtr, &extraOffDiag, sdPtr, wPtr + 1);
U.PostApplyGivens(c, -s, i);
// DEBUG
// DebugCalcBidiagCheck( V, w, superDiag, U );
}
}
void Jacobian::CalcDeltaThetasPseudoinverse()
{
MatrixRmn &J = const_cast<MatrixRmn &>(Jend);
// costruisco matrice J1
MatrixRmn J1;
J1.SetSize(2, J.getNumColumns());
for (int i = 0; i < 2; i++)
for (int j = 0; j < J.getNumColumns(); j++)
J1.Set(i, j, J.Get(i, j));
// COSTRUISCO VETTORI ds1 e ds2
VectorRn dS1(2);
for (int i = 0; i < 2; i++)
dS1.Set(i, dS.Get(i));
// calcolo dtheta1
MatrixRmn U, V;
VectorRn w;
U.SetSize(J1.getNumRows(), J1.getNumRows());
w.SetLength(min(J1.getNumRows(), J1.getNumColumns()));
V.SetSize(J1.getNumColumns(), J1.getNumColumns());
J1.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J1.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
// We multiply by Moore-Penrose pseudo-inverse of the J matrix
double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
long diagLength = w.GetLength();
double *wPtr = w.GetPtr();
dTheta.SetZero();
for (long i = 0; i < diagLength; i++) {
double dotProdCol = U.DotProductColumn(dS1, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
if (fabs(alpha) > pseudoInverseThreshold) {
alpha = 1.0 / alpha;
MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
}
MatrixRmn JcurrentPinv(V.getNumRows(), J1.getNumRows()); // pseudoinversa di J1
MatrixRmn JProjPre(JcurrentPinv.getNumRows(), J1.getNumColumns()); // Proiezione di J1
if (skeleton->getNumEffector() > 1) {
// calcolo la pseudoinversa di J1
MatrixRmn VD(V.getNumRows(), J1.getNumRows()); // matrice del prodotto V*w
double *wPtr = w.GetPtr();
pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
for (int j = 0; j < VD.getNumColumns(); j++) {
double *VPtr = V.GetColumnPtr(j);
double alpha = *(wPtr++); // elemento matrice diagonale
for (int i = 0; i < V.getNumRows(); i++) {
if (fabs(alpha) > pseudoInverseThreshold) {
double entry = *(VPtr++);
VD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
// calcolo la proiezione J1
MatrixRmn::Multiply(JcurrentPinv, J1, JProjPre);
for (int j = 0; j < JProjPre.getNumColumns(); j++)
for (int i = 0; i < JProjPre.getNumRows(); i++) {
double temp = JProjPre.Get(i, j);
JProjPre.Set(i, j, -1.0 * temp);
}
JProjPre.AddToDiagonal(diagMatIdentity);
}
//task priority strategy
for (int i = 1; i < skeleton->getNumEffector(); i++) {
// costruisco matrice Jcurrent (Ji)
MatrixRmn Jcurrent(2, J.getNumColumns());
for (int j = 0; j < J.getNumColumns(); j++)
for (int k = 0; k < 2; k++)
Jcurrent.Set(k, j, J.Get(k + 2 * i, j));
// costruisco il vettore dScurrent
VectorRn dScurrent(2);
for (int k = 0; k < 2; k++)
dScurrent.Set(k, dS.Get(k + 2 * i));
// Moltiplico Jcurrent per la proiezione di J(i-1)
MatrixRmn Jdst(Jcurrent.getNumRows(), JProjPre.getNumColumns());
MatrixRmn::Multiply(Jcurrent, JProjPre, Jdst);
// Calcolo la pseudoinversa di Jdst
MatrixRmn UU(Jdst.getNumRows(), Jdst.getNumRows()), VV(Jdst.getNumColumns(), Jdst.getNumColumns());
VectorRn ww(min(Jdst.getNumRows(), Jdst.getNumColumns()));
Jdst.ComputeSVD(UU, ww, VV);
assert(Jdst.DebugCheckSVD(UU, ww, VV));
MatrixRmn VVD(VV.getNumRows(), J1.getNumRows()); // matrice V*w
VVD.SetZero();
pseudoInverseThreshold = PseudoInverseThresholdFactor * ww.MaxAbs();
double *wwPtr = ww.GetPtr();
for (int j = 0; j < VVD.getNumColumns(); j++) {
double *VVPtr = VV.GetColumnPtr(j);
double alpha = 50 * (*(wwPtr++)); // elemento matrice diagonale
for (int i = 0; i < VV.getNumRows(); i++) {
if (fabs(alpha) > 100 * pseudoInverseThreshold) {
double entry = *(VVPtr++);
VVD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn JdstPinv(VV.getNumRows(), J1.getNumRows());
MatrixRmn::MultiplyTranspose(VVD, UU, JdstPinv);
VectorRn dTemp(J1.getNumRows());
Jcurrent.Multiply(dTheta, dTemp);
VectorRn dTemp2(dScurrent.GetLength());
for (int k = 0; k < dScurrent.GetLength(); k++)
dTemp2[k] = dScurrent[k] - dTemp[k];
// Moltiplico JdstPinv per dTemp2
VectorRn dThetaCurrent(JdstPinv.getNumRows());
JdstPinv.Multiply(dTemp2, dThetaCurrent);
for (int k = 0; k < dTheta.GetLength(); k++)
dTheta[k] += dThetaCurrent[k];
// Infine mi calcolo la pseudoinversa di Jcurrent e quindi la sua proiezione che servirà al passo successivo
// calcolo la pseudoinversa di Jcurrent
Jcurrent.ComputeSVD(U, w, V);
assert(Jcurrent.DebugCheckSVD(U, w, V));
MatrixRmn VD(V.getNumRows(), Jcurrent.getNumRows()); // matrice del prodotto V*w
double *wPtr = w.GetPtr();
pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
for (int j = 0; j < VVD.getNumColumns(); j++) {
double *VPtr = V.GetColumnPtr(j);
double alpha = *(wPtr++); // elemento matrice diagonale
for (int i = 0; i < V.getNumRows(); i++) {
if (fabs(alpha) > pseudoInverseThreshold) {
double entry = *(VPtr++);
VD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
// calcolo la proiezione Jcurrent
MatrixRmn::Multiply(JcurrentPinv, Jcurrent, JProjPre);
for (int j = 0; j < JProjPre.getNumColumns(); j++)
for (int k = 0; k < JProjPre.getNumRows(); k++) {
double temp = JProjPre.Get(k, j);
JProjPre.Set(k, j, -1.0 * temp);
}
JProjPre.AddToDiagonal(diagMatIdentity);
}
//sw.stop();
//std::ofstream os("C:\\buttami.txt", std::ios::app);
//sw.print(os);
//os.close();
// Scale back to not exceed maximum angle changes
double maxChange = 10 * dTheta.MaxAbs();
if (maxChange > MaxAnglePseudoinverse) {
dTheta *= MaxAnglePseudoinverse / maxChange;
}
}
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
