/*! @fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_, const Real dt); @brief Constructor. */ Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_, const Real dt_): dt(dt_), eps(eps_), lambda_max(lambda_max_), mrobot(mrobot_) { robot_type = CLICK_mDH; // Initialize with same joint position (and rates) has the robot. q = mrobot.get_q(); qp = mrobot.get_qp(); qp_prev = qp; Kpep = ColumnVector(3); Kpep = 0; Koe0Quat = ColumnVector(3); Koe0Quat = 0; v = ColumnVector(6); v = 0; if(Kp_.Nrows()==3) Kp = Kp_; else { Kp = DiagonalMatrix(3); Kp = 0.0; cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl; } if(Ko_.Nrows()==3) Ko = Ko_; else { Ko = DiagonalMatrix(3); Ko = 0.0; cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl; } }
void Print(const DiagonalMatrix& X) { ++PCN; cout << "\nMatrix type: " << X.Type().Value() << " ("; cout << X.Nrows() << ", "; cout << X.Ncols() << ")\n\n"; if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; } int nr=X.Nrows(); int nc=X.Ncols(); for (int i=1; i<=nr; i++) { for (int j=1; j<i; j++) cout << "\t"; if (i<=nc) cout << X(i,i) << "\t"; cout << "\n"; } cout << flush; ++PCZ; }
static void tql1(DiagonalMatrix& D, DiagonalMatrix& E) { Tracer et("Evalue(tql1)"); Real eps = FloatingPointPrecision::Epsilon(); int n = D.Nrows(); int l; for (l=1; l<n; l++) E.element(l-1) = E.element(l); Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0; for (l=0; l<n; l++) { int i,j; Real& dl = D.element(l); Real& el = E.element(l); Real h = eps * ( fabs(dl) + fabs(el) ); if (b < h) b = h; int m; for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break; bool test = false; for (j=0; j<30; j++) { if (m==l) { test = true; break; } Real& dl1 = D.element(l+1); Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0); dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h; Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h; p = D.element(m); Real c = 1.0; Real s = 0.0; for (i=m-1; i>=l; i--) { Real ei = E.element(i); Real di = D.element(i); Real& ei1 = E.element(i+1); g = c * ei; h = c * p; if ( fabs(p) >= fabs(ei)) { c = ei / p; r = sqrt(c*c + 1.0); ei1 = s*p*r; s = c/r; c = 1.0/r; } else { c = p / ei; r = sqrt(c*c + 1.0); ei1 = s * ei * r; s = 1.0/r; c /= r; } p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di); } el = s*p; dl = c*p; if (fabs(el) <= b) { test = true; break; } } if (!test) Throw ( ConvergenceException(D) ); Real p = dl + f; test = false; for (i=l; i>0; i--) { if (p < D.element(i-1)) D.element(i) = D.element(i-1); else { test = true; break; } } if (!test) i=0; D.element(i) = p; } }
void getGeneralizedInverse(Matrix& G, Matrix& Gi) { #ifdef DEBUG cout << "\n\ngetGeneralizedInverse - Singular Value\n"; #endif // Singular value decomposition method // do SVD Matrix U, V; DiagonalMatrix D; SVD(G,D,U,V); // X = U * D * V.t() #ifdef DEBUG cout << "D:\n"; cout << setw(9) << setprecision(6) << (D); cout << "\n\n"; #endif DiagonalMatrix Di; Di << D.i(); #ifdef DEBUG cout << "Di:\n"; cout << setw(9) << setprecision(6) << (Di); cout << "\n\n"; #endif int i=Di.Nrows(); for (; i>=1; i--) { if (Di(i) > 1000.0) { Di(i) = 0.0; } } #ifdef DEBUG cout << "Di with biggies zeroed out:\n"; cout << setw(9) << setprecision(6) << (Di); cout << "\n\n"; #endif //Matrix Gi; Gi << (U * (Di * V.t())); return; }
void Clean(DiagonalMatrix& A, Real c) { int nr = A.Nrows(); for (int i=1; i<=nr; i++) { Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; } }
static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z) { Tracer et("Evalue(tql2)"); Real eps = FloatingPointPrecision::Epsilon(); int n = D.Nrows(); Real* z = Z.Store(); int l; for (l=1; l<n; l++) E.element(l-1) = E.element(l); Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0; for (l=0; l<n; l++) { int i,j; Real& dl = D.element(l); Real& el = E.element(l); Real h = eps * ( fabs(dl) + fabs(el) ); if (b < h) b = h; int m; for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break; bool test = false; for (j=0; j<30; j++) { if (m==l) { test = true; break; } Real& dl1 = D.element(l+1); Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0); dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h; Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h; p = D.element(m); Real c = 1.0; Real s = 0.0; for (i=m-1; i>=l; i--) { Real ei = E.element(i); Real di = D.element(i); Real& ei1 = E.element(i+1); g = c * ei; h = c * p; if ( fabs(p) >= fabs(ei)) { c = ei / p; r = sqrt(c*c + 1.0); ei1 = s*p*r; s = c/r; c = 1.0/r; } else { c = p / ei; r = sqrt(c*c + 1.0); ei1 = s * ei * r; s = 1.0/r; c /= r; } p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di); Real* zki = z + i; Real* zki1 = zki + 1; int k = n; while (k--) { h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h; zki += n; zki1 += n; } } el = s*p; dl = c*p; if (fabs(el) <= b) { test = true; break; } } if (!test) Throw ( ConvergenceException(D) ); dl += f; } for (int i=0; i<n; i++) { int k = i; Real p = D.element(i); for (int j=i+1; j<n; j++) { if (D.element(j) < p) { k = j; p = D.element(j); } } if (k != i) { D.element(k) = D.element(i); D.element(i) = p; int j = n; Real* zji = z + i; Real* zjk = z + k; while (j--) { p = *zji; *zji = *zjk; *zjk = p; zji += n; zjk += n; } } } }